Embed Size (px)
Citation preview
UNIVERSITAT POLITÈCNICA DE CATALUNYA
Màster:
AUTOMÀTICA I ROBÒTICA
Tesi de Màster
CALIBRACIÓN AUTOMÁTICA DE LA ODOMETRÍA Y LOS PARÁMETROS DE UN SENSOR
LÁSER DE FORMA SIMULTÁNEA
Pablo García Martínez
Director: Juan Andrade Cetto
Curs Acadèmic 2009/10
Setembre 2010
RESUMEN
El objetivo general que se ha perseguido con este Trabajo Final de Máster ha sido el desarrollo e
implementación de un método de calibración automática para robots móviles.
Para un robot móvil con direccionamiento diferencial equipado con un telémetro láser, existen seis
parámetros para calibrar: tres pertenecientes a la odometría (radios de las ruedas y distancia entre éstas) y
tres para la posición del sensor láser con respecto al eje de referencia del robot.
En el documento “Simultaneous maximum-likelihood calibration of odometry and sensor
parameters” [a] se propone un método para calibrar esos seis parámetros al mismo tiempo, sin la necesidad
de utilizar sensores o dispositivos externos. Además, no es necesario que el robot lleve una determinada
trayectoria y, al contrario que en otros métodos de calibración, no se requerirá la intervención humana,
siendo así un proceso totalmente automático.
Antes de iniciar el proceso de calibración fue necesario recoger los datos procedentes de los encoders
u odometría y las lecturas del sensor láser, tarea que se realiza mientras el robot se desplaza mediante un
algoritmo de navegación. Los datos recogidos deben ser procesados antes de ser utilizados, aplicándoles
técnicas de scan-matching e interpolación de datos. Una vez realizadas estas tareas, se combinan los datos
anteriores para calcular y seleccionar los que realmente utilizará el algoritmo de calibración: las velocidades
angulares de las ruedas, los parámetros devueltos por el algoritmo de scan-matching y la duración del
intervalo. Por último, se aplican una serie de filtros que limpiarán tales datos de medidas erróneas y
“outliers”, permitiendo de este modo obtener unos resultados mucho más precisos. Con los datos anteriores
ya es posible realizar el proceso de calibración y obtener una estimación de los parámetros del robot.
Las pruebas, realizadas tanto en simulación como en robots reales, se efectuaron con un gran número
de configuraciones disponibles en el sistema, por lo que los resultados obtenidos fueron muy variados y nos
permitieron obtener diversas conclusiones apoyándonos en estas.
La implementación se creó utilizando el lenguaje de programación C++. Además, se utilizaron un
gran número de librerías externas que facilitaron la tarea del programador y mejoraron con creces la
funcionalidad y comportamiento del sistema.
AGRADECIMIENTOS
A mi familia, por su continua motivación y apoyo en todo momento.
A Juan Andrade, por sus aportaciones y consejos durante la realización de este trabajo.
A Ricardo y Luca, por toda la ayuda ofrecida en este proyecto.
A Pal-Robotics, por haberme brindado la oportunidad de realizar un proyecto de estascaracterísticas.
A mis amigos de Albacete y Barcelona por estar siempre ahí.
Índice generalCapítulo 1:Introducción...................................................................................................................... 11
1.1Presentación del tema a investigar............................................................................................ 111.2Objetivos y límites del proyecto............................................................................................... 12
Capítulo 2:Aspectos introductorios a la Robótica Móvil .................................................................. 142.1¿Qué son los robots móviles?................................................................................................... 142.2Modos de locomoción en robótica móvil................................................................................. 152.3Modelo cinemático de robots móviles...................................................................................... 15
2.3.1Direccionamiento diferencial ........................................................................................... 172.4Odometría................................................................................................................................. 19
2.4.1Tipos de errores odométricos............................................................................................ 212.5Sensores utilizados en robótica móvil...................................................................................... 22
2.5.1Clasificación de sensores.................................................................................................. 222.5.2Sensores de interés en este proyecto................................................................................. 24
Capítulo 3: Entorno de trabajo........................................................................................................... 263.1Robots móviles utilizados en las pruebas................................................................................. 26
3.1.1Plataformas pertenecientes a Pal-Robotics....................................................................... 263.1.2Modelos utilizados en el simulador Gazebo..................................................................... 29
3.2Software utilizado..................................................................................................................... 313.2.1Sistema Operativo, C++ y entorno de programación........................................................313.2.2Simuladores: Player, Stage y Gazebo .............................................................................. 323.2.3Representación de resultados con Scilab.......................................................................... 343.2.4Creación de diagramas con DIA ...................................................................................... 34
3.3Desarrollos previos................................................................................................................... 353.3.1Formatos para la creación de logs: JSON y CARMEN.................................................... 353.3.2Técnicas de scan-matching: PL-ICP................................................................................. 363.3.3Librerías externas: Eigen, CSM y JSON-Spirit................................................................ 373.3.4Librerías pertenecientes a Pal-Robotics............................................................................ 38
Capítulo 4: Calibración de Robots Móviles. Desarrollos de la investigación.................................... 394.1Análisis de la problemática.......................................................................................................39
4.1.1Calibración de los parámetros físicos del robot ............................................................... 404.1.2Calibración simultánea de la odometría y sensores.......................................................... 41
4.2Diseño del sistema.................................................................................................................... 414.2.1Estudio del documento “Simultaneous maximum-likelihood calibration of odometry and sensor parameters”.................................................................................................................... 424.2.2Cálculo y almacenamiento de los datos de la odometría y del láser.................................474.2.3Preprocesado de los datos ................................................................................................ 514.2.4Fusión de los datos de odometría y CSM......................................................................... 574.2.5Detección y eliminación de datos erróneos.......................................................................594.2.6Modos de navegación del robot........................................................................................ 614.2.7Modos de comportamiento................................................................................................624.2.8Modos de calibración........................................................................................................ 634.2.9Criterios de parada............................................................................................................ 644.2.10Ejecución del método......................................................................................................65
4.3Implementación del programa.................................................................................................. 684.3.1Diagrama de clases............................................................................................................684.3.2Ejecución del programa.....................................................................................................704.3.3Utilización de varios hilos de ejecución............................................................................714.3.4Clase ReemCalibrator....................................................................................................... 714.3.5Clase cal_data................................................................................................................... 754.3.6Clase cal_method.............................................................................................................. 81
4.3.7Clase tools......................................................................................................................... 84Capítulo 5: Pruebas y resultados........................................................................................................ 86
5.1Pruebas en simulador................................................................................................................ 865.1.1Convergencia de los resultados con el paso del tiempo....................................................875.1.2Pruebas con los distintos modos de calibración: Incremento y partición.........................875.1.3Pruebas utilizando el algoritmo de calibración iterativo y el algoritmo de calibración estándar..................................................................................................................................... 895.1.4Resultados obtenidos cambiando los parámetros del Filtro de “outliers”.........................915.1.5Comparación entre distintos modos de navegación: obstacleAvoider y rutas predefinidas...................................................................................................................................................925.1.6Resultados en modo Online: Criterio de parada............................................................... 93
5.2Pruebas con robots reales y comparación con los resultados de simulación............................94Capítulo 6: Conclusiones y trabajos futuros.......................................................................................97
6.1Conclusiones.............................................................................................................................976.2Trabajos futuros........................................................................................................................ 97
Índice de figurasFigura 2.1: Modelo cinemático del direccionamiento diferencial.....................................................17Figura 2.2: Robot móvil con direccionamiento diferencial................................................................20Figura 2.3: Modelo de encoder óptico................................................................................................24Figura 2.4: a) Sensor láser con espejo rotatorio; b) Láser Hokuyo.................................................... 25Figura 3.1: Robots móviles pertenecientes a Pal-Robotics: a) REEM-H1; b) REEM-H2.................28 Figura 3.2: Modelos en Gazebo: a) REEM-H1; b) REEM-H2......................................................... 30Figura 3.3: Entornos 3D: a) oficinas de Pal-Robotics; b) zona de pruebas........................................31Figura 3.4: Proceso PL-ICP: a) Configuración incial; b) Iteración1; c) Iteración2; d) Iteración3; e) Iteración4; f) Iteración5; g) Iteración6; h) Iteración7.........................................................................37Figura 4.1: La posición del robot es qk con respecto a las coordenadas del mundo. La posición del sensor es l con respecto al eje del robot. Ok es el desplazamiento del robot , y sk es el desplazamiento del sensor.................................................................................................................. 43Figura 4.2: Algoritmo CSM (PL-ICP): a) modo Log; b) modo Online..............................................53Figura 4.3: Interpolación lineal.......................................................................................................... 54Figura 4.4: Ejemplo de interpolación de datos................................................................................... 55Figura 4.5: Diagrama de actividad del proceso de interpolación de los datos de odometría.............56Figura 4.6: Intervalos finales..............................................................................................................57Figura 4.7: Fusión de los datos de odometría y CSM........................................................................ 58Figura 4.8: Calculo de los errores entre los desplazamientos calculados por odometría y por CSM 61 Figura 4.9: Modos de calibración: a) incremento; b) partición......................................................... 64Figura 4.10: Diagrama de actividad de la ejecución en modo Log.................................................... 66Figura 4.11: Diagrama de actividad de la ejecución en modo Online................................................67Figura 4.12: Diagrama de clases de la implementación..................................................................... 69Figura 4.13: Algoritmo de la función find_closest() ........................................................................ 80 Figura 4.14: Gráficas de evolución de lo parámetros de calibración............................................... 84Figura 5.1: Comparación de los resultados obtenidos con el algoritmo de calibración iterativo y estándar en el test1..............................................................................................................................91Figura 5.2: Errores de los resultados variando el threshold del Filtro de “outliers”..........................92
Índice de tablasTabla 2.1:Clasificación de sensores....................................................................................................23 Tabla 3.1: Especificaciones técnicas de REEM-H1.......................................................................... 28 Tabla 3.2: Especificaciones técnicas de REEM-H2.......................................................................... 29Tabla 4.1:Muestra de un objeto perteneciente a un log de odometría en formato JSON...................50Tabla 4.2:Muestra de un objeto perteneciente a un log de láser en formato CARMEN....................51Tabla 4.3:Ejemplo de un elemento de un log de CSM....................................................................... 52Tabla 5.1:Parámetros nominales de REEM-H1..................................................................................87Tabla 5.2: Parámetros nominales de REEM-H2.................................................................................87Tabla 5.3: Prueba de larga duración .................................................................................................. 87Tabla 5.4:Pruebas utilizando el modo incremento y el modo partición.............................................88Tabla 5.5:Pruebas utilizando el modo incremento y el modo partición (2)........................................88Tabla 5.6:Pruebas utilizando modo iterativo o no.............................................................................. 90Tabla 5.7:Pruebas utilizando modo iterativo o no (2)........................................................................ 90Tabla 5.8:Pruebas cambiando los parámetros de configuración del Filtro de “outliers”...................91Tabla 5.9: Comparación entre distintos modos de navegación.......................................................... 93Tabla 5.10:Comparación entre distintos modos de navegación......................................................... 93
Capítulo 1: Introducción
En este primer capítulo se realizará una breve introducción de la temática sobre la que versa este
Trabajo Final de Máster, de los objetivos que se deben alcanzar y de las propias limitaciones que acotan la
investigación.
1.1 Presentación del tema a investigar
La robótica es una ciencia que cada día avanza más deprisa y pronto será algo cotidiano el trabajo
conjunto de los humanos con este tipo de máquinas. De hecho, los robots industriales son ya una realidad y
se encuentran totalmente insertados en nuestra sociedad. Existen también otros tipos de robots, como los
denominados robots servicios, que actualmente continúan en estado de investigación e incluso algunas
empresas ya se han aventurado en su comercialización. Un ejemplo de estos son los robots móviles, que
serán el eje central de esta investigación.
Los robots móviles, el tipo de robot que nos interesa, poseen unas características físicas de las que
dependerán factores como la forma de desplazamiento. Además, estos suelen incorporar unos sensores que
les permiten obtener información del entorno, información muy útil a la hora de realizar tareas como la de
navegación. Es por ello que, tanto los parámetros físicos de la plataforma como los de sus sensores, deben
estar configurados correctamente. A la tarea de estimar tales parámetros se le llama calibración y resulta
fundamental en la utilización de estos tipos de robots.
Los robots que se van a estudiar poseen un modelo cinemático con un direccionamiento diferencial y
poseen un sensor láser. Su calibración consiste en estimar los valores de seis parámetros: tres pertenecientes
a la odometría (radios de las ruedas y distancia entre éstas) y tres para la posición del sensor láser con
respecto al eje de referencia del robot. La forma más sencilla y habitual de realizar estos cálculos es manual,
método con el que se producen muchos errores en los cálculos, además de resultar poco práctico. Por otra
parte, existen métodos en los que el robot es capaz de trabajar de forma semiautomática con una intervención
suplementaria del hombre, con los que es posible realizar la calibración obteniendo resultados mucho más
precisos que con el método anterior. También se han desarrollado estrategias para abordar la calibración de
forma totalmente automática, pero un reto mayor consiste en realizar esta tarea tratando de forma simultánea
los parámetros del robot junto a los de un sensor. Una técnica de este tipo es la que se propone en [a], en
concreto para la calibración de la odometría y de un telémetro láser.
Las propuestas teóricas son la base de cualquier proceso de investigación, pero igual de importante
es la capacidad de probar tales propuestas en un entorno real. Plasmar un modelo matemático en un lenguaje
de programación y que funcione tal y como propone el método teórico no es una tarea fácil, y más teniendo
en cuenta los factores externos que entran en juego en robótica móvil, como son los distintos tipos de fuerzas
que existen en el mundo real o los aspectos electrónicos o mecánicos que no se contemplan en el modelo
teórico.
Por tanto, la realización de una implementación del método citado se convierte en un reto que a
priori ofrece un conjunto de características que personalmente resultan de gran interés.
1.2 Objetivos y límites del proyecto
Con el desarrollo de este proyecto de investigación “Calibración Automática de la Odometría y los
Parámetros de un Sensor Láser de forma Simultánea” se pretende crear una implementación del método
descrito en [a] que pueda ser utilizada para calibrar robots móviles con un direccionamiento diferencial y
equipado con un sensor láser. Con esta implementación, además de plasmar el modelo matemático del
documento anterior, deberán de realizarse otras tareas como la recogida de datos, su procesado, y filtrado de
medidas erróneas o imprecisas, entre otras.
De manera genérica, podemos decir que, se pretende investigar cuáles son los problemas propios de
la robótica móvil y la calibración de robots; aplicar estos conocimientos para la creación de una
implementación que permita demostrar, tanto en un simulador como en un entorno de pruebas real, el
correcto funcionamiento del sistema en general y los beneficios que éste ofrece.
Además del objetivo general, anteriormente referido, a continuación se explicitarán los objetivos
más específicos del proyecto donde se concreta con detalle lo que se quiere investigar.
• Objetivo 1.- Estudio sobre robótica móvil: Se investigarán diversos aspectos sobre el estado actual
de la robótica móvil: el modelo cinemático de robots con direccionamiento diferencial, odometría,
tipos de sensores, etc.
• Objetivo 2.- Estudio de distintas propuestas para la calibración de robots móviles: Se revisarán
los métodos existentes utilizados para la estimación de los parámetros físicos de robots móviles con
direccionamiento diferencial (longitud de los radios de las dos ruedas y distancia entre éstas) y para
los parámetros de la posición de un sensor láser con respecto al eje de coordenadas del robot
(parámetros de traslación x e y y de rotación el ángulo de giro theta), de forma individual y
simultánea.
• Objetivo 3.- Familiarización con las plataformas móviles a utilizar: Se examinarán las
características de los robots móviles que se utilizarán en este trabajo: características físicas, modelo
cinemático, sensores, interfaces de comunicación, etc.
• Objetivo 4.- Estudio y comprensión de [a]: Se analizará en profundidad el documento
“Simultaneous maximum-likelihood calibration of odometry and sensor parameters”.
• Objetivo 5.- Familiarización con el entorno de trabajo: En este proyecto se trabajará con distintos
tipos de software, tales como simuladores o analizadores sintácticos, por lo que será necesario
obtener un buen entendimiento de ellos para más tarde poder utilizarlos. También se analizarán las
librerías externas utilizadas en la implementación para tratar temas como la comunicación entre el
robot y nuestro programa, realizar operaciones matemáticas complejas, tareas de navegación del
robot, etc.
• Objetivo 6.- Implementación del algoritmo descrito en [a]: Se creará un programa que
implemente el algoritmo de calibración citado en el Objetivo 4.
• Objetivo 7.- Implementación de métodos que permitan la recepción de datos a través de los
sensores de un robot móvil y su procesamiento: Se utilizarán una serie de interfaces con las que se
podrán establecer una comunicación directa entre el robot y nuestro programa, permitiendo la
recepción de lecturas procedentes de la odometría y del telémetro láser. Además, se crearán métodos
para el procesamiento de los datos, requerimiento para poder utilizar el algoritmo de calibración.
También se desarrollarán técnicas con las que poder filtrar “outliers” y lecturas incorrectas, con el fin
de obtener resultados más precisos.
• Objetivo 8.- Creación de distintas configuraciones para el sistema: Se ampliarán las
posibilidades del método con diferentes opciones de configuración, donde se podrá elegir entre
distintos modos navegación, de comportamiento, o incluso la forma en la que se aplica el algoritmo
de calibración.
• Objetivo 9.- Realización de pruebas con distintas configuraciones: Se realizarán una serie de
pruebas, tanto en robots reales como en simulación, para comprobar el correcto funcionamiento del
método. Cambiando las configuraciones citadas en el Objetivo 8 se podrán sacar diversas
conclusiones, conocer la precisión del método y encontrar una configuración genérica para cualquier
robot y situación.
Sobre los límites del proyecto
También será necesario precisar los límites del propio proyecto. En primer lugar, no se pretende crear
un nuevo método de calibración, sino realizar una implementación de una propuesta teórica ya existente,
aunque realizando algunas mejoras que a priori aumentarán la eficiencia del método y reducirán el tiempo
empleado en la calibración. Tampoco se pretende mejorar los resultados obtenidos de forma concluyente, ya
que el modelo matemático es el mismo que el descrito en [a], aunque sí pueden obtenerse resultados
ligeramente más precisos debido a la aplicación de métodos de limpieza y filtrado de “outliers”.
Capítulo 2: Aspectos introductorios a la Robótica Móvil
En este capítulo se realizará una introducción al estado del arte sobre la robótica móvil; se definirán
algunos conceptos para mejorar su comprensión y se abordarán temas, capitales para este proyecto, como
modelo cinemático, odometría y tipos de sensores.
2.1 ¿Qué son los robots móviles?
Para comenzar se revisará un concepto básico, ¿qué es la robótica? Cuando pensamos en un robot, lo
primero que nos viene a la cabeza es una máquina con forma o rasgos humanos que se comporta e
interacciona con el mundo imitando la forma en que lo hacemos nosotros. Este concepto viene dado por la
literatura de ciencia ficción que, desde hace varias décadas, ha imaginado un mundo lleno de robots capaces
de ayudarnos y servirnos, otorgándoles incluso características puramente humanas como los sentimientos,
autoconsciencia y una inteligencia equivalente a la humana.
A continuación se presentarán algunas definiciones de robot:
a) La Real Academia Española [w12] define robot como: “Máquina o ingenio electrónico
programable, capaz de manipular objetos y realizar operaciones antes reservadas sólo a las
personas.”
b) La definición adoptada por el Instituto Norteamericano de Robótica [w13] aceptada
internacionalmente para robot es: “Manipulador multifuncional y reprogramable, diseñado para
mover materiales, piezas, herramientas o dispositivos especiales, mediante movimientos
programados y variables que permiten llevar a cabo diversas tareas.”
Por otra parte, el término robótica fue acuñado por Isaac Asimov, científico y escritor prestigioso,
definiéndola como la Ciencia que estudia a los robots. La Real Academia Española [w12] define robótica
como: “Técnica que aplica la informática al diseño y empleo de aparatos que, en sustitución de personas,
realizan operaciones o trabajos, por lo general en instalaciones industriales.”
Tal y como se comenta en [e], la robótica ha conseguido su mayor logro hasta la fecha en el mundo
de la industria. Los brazos robóticos o manipuladores se pueden mover con gran velocidad y precisión para
realizar tareas repetitivas tales como soldaduras o pinturas. Aún así, estos robots comerciales poseen una
desventaja fundamental, la falta de movilidad. Un manipulador fijo tiene un rango de movimiento limitado
dependiendo de donde se encuentre situado, sin embargo, un robot móvil podría ser capaz, por ejemplo, de
viajar por toda la planta de producción, aplicando sus cualidades donde y cuando sea requerido.
Así, con la utilización de robots móviles se abre un gran rango de aplicaciones. Las zonas de difícil
acceso o riesgo para los humanos es una de ellas: exploraciones no tripuladas a planetas lejanos, plantas
nucleares o químicas, vehículos submarinos, etc. Otros robots móviles no son creados para acceder a zonas
complicadas, sino para convivir con el hombre y ayudarle en diferentes tareas: robots de servicios capaces de
informar o guiar a personas en museos o centros comerciales, de limpiar hogares de forma autónoma, robots
bélicos que ayuden en tareas peligrosas en misiones militares, etc.
Con todo esto, la creación de robots móviles requiere el estudio y diseño de una serie de factores
específicos para cada tipo de robot. En los siguientes apartados se abordarán sus principales características y
se profundizará sobre las que resultan de interés para este proyecto.
2.2 Modos de locomoción en robótica móvil
Los robots móviles requieren de mecanismos de locomoción para permitir su movimiento por
diferentes entornos. Existe una gran variedad de posibles formas de movimiento, por ello la elección del tipo
correcto de locomoción es un aspecto fundamental en el diseño de robots móviles.
Una posible clasificación de los tipos de locomoción podría ser ésta, tal y como se presenta en [h]:
a) Vehículos con ruedas: La rueda es un invento humano que ofrece una gran eficiencia en terrenos
planos. Los vehículos que las utilizan son la solución más simple y eficiente para conseguir
movilidad en superficies lo suficiente duras y libres de obstáculos, permitiendo conseguir
velocidades relativamente altas. Una de las limitaciones más significativas es el deslizamiento en la
impulsión, pudiéndose presentar también deslizamientos y vibraciones dependiendo de las
características del terreno. Además, resultan ser poco eficiente en terrenos blandos. Existen diversas
configuraciones entre las que destacan: Ackerman, triciclo clásico, direccionamiento diferencial, skid
steer, pistas de deslizamiento y síncrona. Este trabajo se centra en los robots móviles con
direccionamiento diferencial, por lo que en apartados posteriores se profundizará en éstos.
b) Locomoción mediante patas: Este modo de locomoción permite aislar el cuerpo del terreno
utilizando únicamente puntos discretos de soporte, tienen mejores propiedades que las ruedas para
atravesar terrenos complejos con muchos obstáculos, permite movimientos omnidireccionales y
además ofrece menos deslizamiento en la locomoción. En estos tipos de robots la complejidad de los
mecanismos es mucho mayor, al igual que el consumo de energía. Además, los problemas de
planificación y control resultan más complicados que en los vehículos con ruedas.
c) Robots submarinos y aéreos: Los robots submarinos y aéreos han acogido un gran interés debido a
su aplicación en tareas como inspección, recogida de datos o mantenimiento de instalaciones en
entornos naturales a los que el acceso de los humanos resulta de gran complejidad. Además, estos
vehículos suelen ser el resultado de la evolución de los vehículos teleoperados por el hombre.
Otro tipo de clasificación posible podrían ser los robots móviles basados en inspiraciones biológicas
[e]. Observando comportamientos existentes en la naturaleza, el hombre ha intentado imitar y aplicar tal
conocimiento a los robots, otorgándoles la posibilidad de desplazarse arrastrándose, reptando, corriendo,
saltando o incluso andando sobre dos piernas, tal y como lo hace el ser humano. Éste último presenta algunas
complicaciones extras, como lo es el mantenimiento del equilibrio. En este sentido se recogen dos tipos de
equilibrio: estático, en el que el robot es capaz de mantener el equilibrio sin desplazarse ni moverse, y
dinámico, donde al contrario que en el anterior el robot debe moverse de forma continua.
2.3 Modelo cinemático de robots móviles
Tal y como se explica en [c], existen tres factores fundamentales a la hora de calcular el modelo
cinemático de un robot móvil:
a) Locomoción: Es el proceso que causa el movimiento en un robot autónomo. Para producir tal
movimiento, una serie de fuerzas deben ser aplicadas al vehículo.
b) Dinámica: Es el estudio del movimiento en el cual las fuerzas utilizadas en la locomoción son
modeladas. Incluye las energías y velocidades asociadas a tales movimientos.
c) Cinemática: Es el estudio del modelo matemático del movimiento sin considerar las fuerzas que
afectan al movimiento (dinámica). Trata las relaciones geométricas que conforman el sistema y la
relación entre el control de parámetros y el comportamiento de un sistema en el espacio de
configuraciones.
Según [d], para el cálculo del modelo cinemático de un robot móvil se deben de cumplir una serie
de hipótesis básicas:
a) El robot se mueve sobre una superficie plana.
b) Los ejes de guiado son perpendiculares al suelo.
c) El deslizamiento es despreciable en el período de control.
d) El robot no tiene partes flexibles.
e) Durante un período de tiempo suficientemente pequeño el vehículo se moverá de un punto al
siguiente a lo largo de un arco de circunferencia llamado ICC (Instantaneous Center of Curvature).
f) El robot se comporta como un sólido rígido de forma que, si existen partes móviles, éstas se situarán
en la posición adecuada mediante el sistema de control.
Además, tal y como se explica en [g], los robots móviles con ruedas se pueden clasificar en dos tipos
básicos, holonómicos y no-holonómicos:
a) Holonómicos: Se denomina así a los robots móviles que no tiene ningún tipo de restricción en las
velocidades aplicadas para desplazarse en un plano 2D. Los robots móviles holonómicos poseen un
máximo de tres grados de libertad sobre un plano 2D, dos para la posición y uno para la orientación,
generalizando así el término de vehículo holonómico a aquellos con este número de grados de
libertad. Para poder lograr este movimiento holonómico se siguen varias estrategias, como la
utilización de ruedas omnidireccionales, ortogonales o ruedas esféricas, permitiendo una
planificación de movimiento sobre un plano de forma sencilla.
b) No holonómicos: Los vehículos móviles no holonómicos imponen restricciones sobre las
velocidades para mover al robot sobre un plano, teniendo éstos menos de tres grados de libertad. Aún
así son más simples en construcción y por tanto más baratos, con menos ejes controlables y aseguran
la movilidad necesaria sobre el plano. Dadas estas características, los robots móviles más utilizados
son los de este tipo (dos grados de libertad), incluyendo los utilizados en este proyecto en esta
categoría.
Tal y como se comentó anteriormente, existen diversos tipos de configuraciones: síncrona,
direccionamiento diferencial, triciclo clásico y bicicleta, etc. A continuación se mostrarán las principales
características de los robots móviles con direccionamiento diferencial, el tipo de robot móvil en el que se
centra este proyecto.
2.3.1 Direccionamiento diferencial
El mecanismo del modelo de direccionamiento diferencial [k] consiste en dos ruedas no directrices
montada sobre un eje común, existiendo la posibilidad de mover cada una de las ruedas de forma
independiente para realizar movimientos marcha adelante o marcha atrás. Variando las velocidades de ambas
ruedas se puede también modificar la trayectoria que lleva el robot.
Como se explica de [d], en este tipo de modelo cinemático las variables de control son las
velocidades de las ruedas laterales ωi y ωd. Considerando el radio de las ruedas r, las velocidades lineales
correspondientes son:
υi = ωi*r (2.1)
υd = ωd*r (2.2)
A continuación se detallará el proceso para el cálculo del modelo cinemático en función del eje de
coordenadas de referencia: global o local.
Modelo cinemático trabajando en coordenadas del mundo (eje de coordenadas global)
Las velocidades lineal y angular del modelo vienen dadas por
υ=υdυi
2=
ωiωd ∗r
2 (2.3)
ω=υdυi
b=
ωd−ωi∗r
b (2.4)
donde b es la distancia que separa ambas ruedas.
Si la tarea viene especificada por υ y ω, entonces será necesario convertir a ωi y ωd
ωi=υ−b/ 2∗ω
r (2.5)
ωd=υb/2∗ω
r (2.6)
Sustituyendo υ y ω en
[ẋẏθ ]=[−sen 0cos 0
0 1] [ υω] (2.7)
obtenemos el Jacobiando del direccionamiento diferencial
[ẋẏθ ]=[− r∗sen /2 − r∗sen r∗cos/2 r∗cos/2
−c /b c/b ] [ωi
ωd] (2.8)
Modelo cinemático trabajando en coordenadas del robot (eje de coordenadas local)
Esta forma de calcular el modelo cinemático resulta muy útil sobre todo para el control de velocidad.
[V x t V y t θ t ]=[ r /2 r /2
0 0−r / L r / L] [ωlt
ωr t ] (2.9)
Tal y como se explica en [k], el robot gira sobre un punto que se encuentra en el eje común de ambas
ruedas, conocido como ICC (Instantaneous Center of Curvature) o centro instantáneo de curvatura (ver
figura 2.1).
Se tienen las variables de control
Vr: velocidad lineal de la rueda derecha
Vl: velocidad lineal de la rueda izquierda
y otro conjunto de variables
r: radio nominal de cada una de las ruedas
b: distancia entre los centros de las dos ruedas
R: distancia con signo desde ICC hasta el punto medio entre ambas ruedas
Además, tal y como se observa en la figura 2.1, se tiene que
R – L/2: Radio de curvatura de la trayectoria descrita por la rueda izquierda
R + L/2: Radio de curvatura de la trayectoria descrita por la rueda derecha
Ya que el grado de rotación ω sobre ICC debe ser el mismo para ambas ruedas, se pueden escribir las
siguientes ecuaciones:
ω (R + l/2) = Vr (2.10)
ω (R − l/2) = Vl (2.11)
En un instante de tiempo dado se puede resolver R y ω como
R = l/2*(Vl + Vr )/(Vr − Vl ) (2.12)
ω = (Vr − Vl )/l (2.13)
Existen tres casos particulares que resultan interesantes en este tipo de modelo cinemático:
a) Si Vl = Vr = V, entonces se tiene un movimiento rectilíneo del vehículo. R se convierte en infinito,
por lo que no existe rotación (ω = 0).
b) Si Vl = -Vr, entonces R = 0 con ICC en el punto medio de las ruedas, obteniendo una rotación sobre
el centro medio del eje de las ruedas del robot (se obtiene una rotación sin desplazamiento).
c) Si Vl = 0, entonces se tiene una rotación sobre la rueda izquierda, donde R = l/2. Se contempla la
misma situación en el caso de Vr = 0.
2.4 Odometría
La odometría [o] es el método más utilizado para determinar la posición de un robot en un momento
dado. En muchas aplicaciones esta técnica ofrece una forma fácil de acceder en tiempo real a cierta
información, como la posición absoluta del robot, dependiendo la precisión de este sistema de la frecuencia
con la que estos datos son calculados.
Como se observa en la figura 2.2, los encoders, acoplados a los motores, cuentan las revoluciones de
las ruedas y, utilizando una serie de ecuaciones geométricas, se puede calcular de forma sencilla una
estimación de la posición actual del vehículo relativa a una posición inicial conocida. Determinar los errores
odométricos de un robot móvil resulta de vital importancia, tanto para reducirlos como para conocer la
precisión de la configuración del robot.
Para un mejor entendimiento se desarrollarán las ecuaciones odométricas propuestas en [i]:
Suponemos que en un intervalo I los encoders de las ruedas derecha e izquierda muestran un
incremento de pulsos de NL y NR, respectivamente. Suponemos además que:
cm = πDn/nCe (2.14)
donde
cm - Factor de conversión que transforma los pulsos de los encoders en desplazamiento lineal de la
rueda.
Dn - Diámetro nominal de la rueda (en mm).
Ce - Resolución del encoder (en pulsos por revolución).
n - Relación de cambio entre el motor (donde se encuentra acoplado el encoder) y la rueda.
Ahora se puede calcular la distancia incremental recorrida por la rueda izquierda y derecha, ΔUL, I y
ΔUR, I , de acuerdo a
ΔUL/R, I = cm NL/R, I (2.15)
y el desplazamiento incremental lineal del centro del robot robot C, denotado como ΔUi , de acuerdo a
ΔUi = ( ΔUR,i + ΔUL,i)/2 (2.16)
Para calcular el cambio incremental de la orientación del robot se utilizará
Δ θ i = ( ΔUR,i – ΔUL,i)/b (2.17)
donde b es la distancia entre las ruedas del vehículo, idealmente medida como la distancia entre los dos
puntos de contacto entre las ruedas y el suelo.
La nueva orientación relativa del robot θi puede ser calculada como
θi = θi-1 + Δθi (2.18)
y la posición relativa del punto central del robot es
xi = xi-1 + ΔUi cos θi (2.19a)
yi = yi-1 + ΔUi sin θi (2.19b)
donde
xi, yi – posiciones relativa del punto central del robot c en el instante I.
Como se puede observar en (2.14) – (2.19), la odometría está basada en ecuaciones que se
implementan de forma sencilla y que utiliza datos “baratos” (fáciles de calcular) de los enconders
incrementales de las ruedas. Sin embargo, la odometría se sostiene en la asunción de que las revoluciones de
las ruedas se puede transformar en un desplazamiento lineal relativo con respecto al suelo, siendo ésta de una
validez limitada. Un ejemplo es el deslizamiento de las ruedas; si una rueda desliza debido a, por ejemplo, un
vertido de aceite, entonces el encoder asociado registrará revoluciones en las ruedas aunque estas
revoluciones no se correspondan con un desplazamiento real.
2.4.1 Tipos de errores odométricos
Según [j], los errores odométricos pueden clasificarse en dos tipos: errores sistemáticos y errores no-sistemáticos.
A. Errores sistemáticos
Son causados por imperfecciones en el diseño e implementaciones mecánicas del robot móvil. Las
principales causas son:
a) Diámetros diferentes de las ruedas.
b) La media aritmética de los diámetros de ambas ruedas es diferente del diámetro nominal.
c) Mal alineamiento de las ruedas.
d) Incertidumbre de la distancia entre las dos ruedas.
e) Resolución de los encoders limitada.
f) Muestreo de los encoders limitado.
Según varios autores [Borenstein and Koren, 1985; 1987; Crowley, 1989; Komoriya and Oyama,
1994; Everett, 1995] las dos fuentes principales de problemas debido a errores sistemáticos son el diámetro
diferente entre las ruedas y la incertidumbre entre la distancia de ambas ruedas:
a) Diámetro desigual de las ruedas: Ya que muchos robots móviles utilizan ruedas de goma, resulta
complicada la fabricación de éstas con unas medidas exactamente iguales, en este caso con un
mismo diámetro. Además, este tipo de ruedas se comprimen de forma diferente en distribuciones de
carga de peso desiguales. Ambas causas pueden producir sustanciales errores en el cálculo de la
odometría.
b) Distancia entre las ruedas: Es necesario conocer esta medida con precisión ya que se utiliza para
calcular el número de pulsos diferenciales de los encoders que corresponden con una cierta cantidad
de rotación del vehículo. Esta incertidumbre se debe al hecho de que las ruedas no sufren un
contacto con el suelo en un punto, sino en un área, afectando gravemente en el cálculo de la
odometría.
Se denotarán los errores debidos a una desigualdad de radio entre ruedas como Ed y a una distancia
entre ruedas incorrecta como Eb. Es importante precisar que Eb tiene efecto sólo en los giros, mientras que Ed
sólo afecta al movimiento en línea recta.
Se definen por lo tanto
Ed = Dr/Dl (2.20)
Eb = bactual/bnominal (2.21)
donde Dr y Dl son el diámetro actual de las ruedas y b es la distancia entre las ruedas del vehículo.
Hasta este punto sólo se han definido los errores Eb y Ed. Sin embargo, si la media entre los
diámetros de las ruedas, denotado como Da, difiere del diámetro nominal de las ruedas, entonces el vehículo
sufrirá un error de odometría adicional, que se denotará como Es, afectando en el movimiento rectilíneo del
robot. Aunque este error puede empeorar de forma significativa el cálculo de la odometría, resulta muy
sencillo de calcular con una serie de mediciones utilizando marcas en el suelo. Es por ello que normalmente
se asume que Es ha sido medido correctamente en los procesos de calibración de la odometría.
B. Errores no-sistemáticos
Son aquellos errores causados por la interacción del robot con las características impredecibles del
terreno. Algunos ejemplos son:
a) Desplazamiento sobre terrenos desnivelados.
b) Desplazamiento sobre objetos inesperados sobre el suelo.
c) Deslizamiento de las ruedas.
Estos tipos de errores son un gran problema en las aplicaciones actuales, ya que es imposible de
predecir el error que se va a producir.
2.5 Sensores utilizados en robótica móvil
Existe una gran variedad de sensores utilizados en robótica móvil. Algunos se utilizan para realizar
mediciones sobre el estado del robot, como la temperatura interna de éste o la velocidad rotacional de las
ruedas. Otros más sofisticados pueden ser utilizados para obtener información del medio donde se encuentra
el robot o incluso para obtener la posición global de éste en un mapa.
En este apartado se citarán los sensores típicos en robótica móvil y sus características principales,
profundizando en los utilizados en este trabajo.
2.5.1 Clasificación de sensores
Según [e], los sensores se pueden clasificar basándonos principalmente en dos factores, si son
proprioceptivo/exteroceptivo y pasivos/activos:
a) Proprioceptivo: Este tipo de sensor mide los valores internos del sistema (robot); por ejemplo,
la velocidad de los motores, los ángulos de las articulaciones de un robot o el voltaje de la
batería (nivel de batería).
b) Exteroceptivo: Reciben la información del entorno donde se encuentra el robot; por ejemplo,
medidas de distancias, intensidad de luz, amplitud de sonido. Así, las medidas de este tipo son
interpretadas por el robot para así extraer las características del entorno que resulten de interés.
c) Pasivos: Miden la energía medioambiental que entra en sus sensores. Ejemplos de este tipo son
los micrófonos o las cámaras CCD o CMOS.
d) Activos: Se comportan emitiendo primeramente energía en el ambiente para a continuación
medir las reacciones provocadas en éste. Ya que los sensores activos pueden interaccionar de
forma controlada con el ambiente, normalmente consiguen un mejor comportamiento. Sin
embargo, también tienen algunos inconvenientes: la energía de salida puede afectar a algunas de
las características que se desean medir. Además, pueden sufrir interferencias entre sus propias
señales y otras fuera de su control. Por ejemplo, las señales emitidas por robots vecinos o
sensores similares en una misma plataforma pueden influenciar en el resultado final de la
medición. Sensores de ultrasonidos y sensores láser son algunos ejemplos de sensores activos.
En la tabla 2.1 se muestran los sensores más utilizados y su clasificación siguiendo los factores
anteriores:
Tabla 2.1:Clasificación de sensores
Clasificación general Tipo de sensor PC o EC1 A o P2
Sensores táctiles(detección de contacto físicoo cercanía, interruptores de seguridad)
Bumpers (Interruptores de contacto)Barreras ópticasSensores de proximidad sin
ECECEC
PAA
1 PC, proprioceptivo; EC, exteroceptivo
2 A, activo; P, pasivo; P/A, pasivo/activo
contacto
Sensores utilizados en ruedas/motores(velocidad y posición)
EncodersPotenciometrosEncoders ópticosEncoders magnéticosEncoders inductivosEncoders capacitivos
PCPCPCPCPCPC
PPAAAA
Direccionamiento del robot (orientación del robot con respecto a un eje de referencia fijo)
BrújulasGiróscoposInclinómetros
ECPCEC
PPA/P
Balizas terrestres (localización en un eje de referencia fijo)
GPSBalizas RFBalizas de ultrasonidoBalizas reflectantes
ECECECEC
AAAA
Sensores para el cálculo dedistancias (tiempo de vuelo, triangulación geométrica)
Sensores reflectantesSensores ultrasónicos Telémetro láserTriangulación óptica (1D)Luz estructurada (2D)
ECECECECEC
AAAAA
Sensores de movimiento (velocidad relativa a objetosfijos o en movimiento)
Radar DopplerSonido Doppler
ECEC
AA
Sensores basados en visión(cálculo de distancias, análisis de imágenes, segmentación, reconocimiento de objetos)
Cámaras CCD/CMOS EC P
2.5.2 Sensores de interés en este proyecto
Como se observa en la tabla 2.1, existe una gran cantidad de sensores existentes en robótica móvil.
Aunque las plataformas móviles que se utilizarán en este trabajo (ver apartado 3.1) poseen varios de estos
sensores, únicamente se utilizarán dos tipos: un telémetro láser, con el que se podrán obtener lecturas láser
del entorno y así poder aplicar un algoritmo de scan-matching para estimar la roto-traslación del robot, y los
encoders ópticos de los motores de las ruedas, que ofrecerán información sobre cuánto ha girado una rueda
y, por tanto, las velocidades angulares de éstas.
A continuación, siguiendo las explicaciones en [f] y [e], se expondrán las características principales
de estos dos tipos de sensores.
A. Encoders de los motores
Los encoders de tipo óptico se han convertido en el dispositivo más popular para medir la velocidad
angular y posición de las ruedas en robótica móvil. Debido a que este tipo de sensor es proprioceptivo, tal y
como se indicó en el apartado anterior, la estimación de la posición del robot se realiza de forma más precisa
trabajando en el propio eje de referencia del robot. Cuando se afrontan otros problemas, como el de
localización por medio de odometría (ver apartado 2.4), se deberán realizar correcciones de forma sustancial.
Este tipo de sensor se implementa mediante un disco delgado metálico con una serie de ranuras
equidistantes y homogéneas que se acopla directamente al eje del motor. Éste gira dentro de un switch óptico,
que deja pasar e interrumpe un haz de luz infrarroja, suministrada por un led infrarrojo de la cabeza lectora
fija. Este sistema se completa con un fotodetector, ya sea LDR (celda fotoeléctrica) o fototransistor. El
switch optoelectrónico está formado por tanto, tal y como se muestra en la figura 2.3, por un par de
dispositivos: un emisor y un receptor de luz infrarroja, colocados uno en frente del otro y con una ranura
entre ambos por donde gira libremente el disco dentado montado en el eje de la rueda.
Los encoders resultan imprescindibles en tareas de navegación, ya que permiten, por medio de la
odometría (teniendo en cuenta la acumulación de error que produce esta técnica), conocer la posición
absoluta de un robot móvil.
B. Telémetro láser
Este tipo de sensor es del tipo en tiempo-de-vuelo y con él se consiguen mejorar los resultados
obtenidos por los sensores de ultrasonidos, ya que se utiliza luz láser en vez de sonido. Un dispositivo
telémetro láser (ver figura 2.4a) se compone de en un transmisor que ilumina un objetivo con rayos láser, y
un receptor que es capaz de detectar la componente de luz compatible con la emitida. Un sistema mecánico
con un espejo es el encargado de lanzar rayos de luz para cubrir el espacio en un plano o incluso en tres
dimensiones, utilizando mecanismos que permitan la rotación del espejo.
Un método para estimar la distancia a la que se encuentran los objetos consiste en calcular el tiempo
que un impulso de luz láser tarda en colisionar con un objeto y regresar al receptor, igual que en el caso de
los ultrasonidos. Para su utilización se requieren dispositivos electrónicos muy caros capaces de realizar
cálculos en picosegundos. Un segundo método consiste en medir la frecuencia entre una onda de frecuencia
modulada (FMCW) y su reflejo recibido. Por último, otra alternativa mucho más sencilla se trata de medir el
desplazamiento de fase de la luz reflejada.
El telémetro láser es muy utilizado en técnicas de navegación, ya que permite la creación de mapas
de forma precisa, la localización del robot móvil, o técnicas más avanzadas como SLAM (Simultaneous
Localization and Mapping). En este proyecto se trabajará con un sensor láser Hokuyo (ver figura 2.4b), muy
utilizado debido a sus reducidas dimensiones y peso, su interfaz de comunicación USB y su precio, más
ajustado que el de sus predecesores.
Capítulo 3: Entorno de trabajo
En este capítulo se explicarán las especificaciones técnicas y particularidades de las plataformas
móviles con las que se trabajará en esta Tesis de Máster, así como las principales características de los
programas (software) utilizados. También se realizará una introducción a técnicas, formatos y librerías
externas utilizadas en la fase de implementación.
3.1 Robots móviles utilizados en las pruebas
Para probar la implementación realizada en este trabajo, se utilizaron dos plataformas móviles de
características similares. Éstas pertenecen a la empresa Pal-Robotics y además se crearon unos modelos
virtuales con los que se pudo trabajar en un entorno de simulación.
3.1.1 Plataformas pertenecientes a Pal-Robotics
Este trabajo final de máster fue desarrollado en la empresa Pal-Robotics [w7], dedicada a la
investigación y desarrollo de robots humanoides y componentes robóticos. La mayoría de las pruebas fueron
realizadas con plataformas móviles pertenecientes a ella. Actualmente Pal-Robotics posee cuatro modelos de
robots humanoides: dos de ellos son robots bípedos, REEM-A y REEM-B, y no fueron utilizados para este
proyecto ya que el algoritmo implementado aborda la calibración de robots móviles sobre ruedas con un
modelo cinemático diferencial. Las otras dos plataformas, REEM-H1 y REEM-H2, sí fueron utilizadas en la
fase de pruebas (desarrolladas en el capítulo 5), tanto con sus modelos para el simulador Gazebo como con
las plataformas reales.
Tanto REEM-H1 como REEM-H2 comparten muchas de sus características aunque también difieren
en pequeñas particularidades. Veamos a continuación una breve revisión de las mismas:
a) Mismo modelo cinemático: Ambas plataformas comparten el mismo modelo cinemático, del tipo
direccionamiento diferencial (ya explicado en el apartado 2.3.1).
b) Configuración de las ruedas distinta: REEM-H1 posee el eje de las ruedas motrices alineado con
el punto central del robot y mantiene el equilibrio gracias a cuatro ruedas locas. Por otra parte,
REEM-H2 posee sólo 2 ruedas caster, pero en este caso el eje de las ruedas motrices se encuentra
más adelantado que en el modelo anterior.
c) Mismo tipo de sensores pero en distinto número: Los sensores utilizados en ambas plataformas
son en su mayoría del mismo tipo, aunque en REEM-H2 el número de éstos suele ser mayor; por
ejemplo, éste posee más dispositivos de sonar y dos telémetros láser en vez de uno.
d) Actuadores con diferentes características: Los actuadores, como los motores de las ruedas o de las
articulaciones, poseen una mayor potencia en la plataforma REEM-H2.
Las especificaciones técnicas de ambas plataformas son similares en algunos aspectos. Sin embargo,
REEM-H2, el segundo robot humanoide con base móvil sobre ruedas creado por Pal-Robotics, es una
versión mejorada de REEM-H1 en prácticamente todos los aspectos, tanto en “hardware” como en
“software”. En las tablas 3.1 y 3.2 pueden compararse las especificaciones técnicas de ambas plataformas.
Tabla 3.1: Especificaciones técnicas de REEM-H1
Peso 90 Kg.
Altura 1.65 metros
Autonomía de la batería 8 horas
Grados de libertad 21
Capacidad de carga 6 Kg.
Velocidad 4 Km/h
Ordenador Placa base Mini-ITX con un procesador Core2Duo3Gb de memoria RAM
Sensores 2 cámaras DFK21AF04láser Hokuyo UTM30-LX10 sensores sónar Devantech SR05acelerómetros giróscoposmicrófono
Tabla 3.2: Especificaciones técnicas de REEM-H2
Peso 90 Kg.
Altura 1.70 metros
Autonomía de la batería 8 horas
Grados de libertad 22
Capacidad de carga 30 Kg para la base móvil3 Kg. En cada brazo
Velocidad 4 Km/h
Ordenadores Placa base Mini-ITX con un procesador Core2DuoATOM D410 con 2Gb de memoria RAM (para multimedia)3Gb de memoria RAM
Sensores (los modelos aún no son públicos)
cámara esteroscópicaláserultrasonidosacelerómetrosgiróscoposmicrófono
De entre todas las características de ambas plataformas destacan la mayor capacidad de carga de
peso por parte de REEM-H2 o el segundo ordenador incorporado en la misma plataforma para uso exclusivo
de funciones multimedia (pantalla táctil, sonido, etc).
3.1.2 Modelos utilizados en el simulador Gazebo
Son muchas las ventajas que se obtienen de trabajar en un entorno de simulación y ya que se poseen
todas las herramientas (software) necesarias para trabajar en un entorno de este tipo (ambas afirmaciones
serán explicadas en apartados posteriores), únicamente será necesario crear los modelos de los robots que
representen de forma fiel las principales características de las plataformas móviles reales. Para ello se
crearon dos modelos 3D de los robots REEM-H1 y REEM-H2, donde se tuvieron en cuenta para su
desarrollo sus modelos cinemáticos, sus características físicas y los tipos de sensores y actuadores que éstos
poseen.
En la figura 3.2 se muestran los dos modelos creados. Inicialmente se desarrollaron bajo ROS3, ya
que éste también incluye el simulador Gazebo; pero dado que se querían aprovechar las librerías que Pal-
Robotics poseía para la comunicación con Player/Gazebo, se decidió convertir los modelos en formato urdf
(perteneciente a ROS) a xml (interpretado por Gazebo) y seguir con el desarrollo de éstos exclusivamente
para Gazebo.
Sin entrar demasiado en detalle, para el desarrollo de REEM-H1 (ver figura 3.2a) se utilizaron, para
la mayoría de las partes del robot, una serie de elementos gráficos compuestos por mallas de triángulos
3 (Robot Operating System) “Meta-Sistema Operativo” de código abierto ampliamente utilizado en robótica.
(interpretadas por la librería gráfica OGRE4) que, además de ofrecer una mayor fidelidad visual del modelo,
permite el cálculo de colisiones con diferentes resoluciones dependiendo del tamaño y número de triángulos
utilizados. Por otra parte, para el modelo REEM-H2 no se disponía de los elementos gráficos utilizados en el
caso anterior, por lo que se utilizaron figuras geométricas básicas tales como esferas, cubos o cilindros.
En apartados anteriores se mostraron las principales características de ambas plataformas, junto al
tipo y número de sensores que éstas poseen. Los sensores creados para los modelos de Gazebo son los
ultrasonidos, telémetros láser y cámaras estereoscópicas, aunque para este proyecto únicamente se utilizará el
sensor láser. Otros tipos sensores, como los acelerómetros o giróscopos, no se incluyeron en los modelos ya
que, principalmente, el simulador no ofrecía la posibilidad de trabajar con ellos.
En estos modelos, además del diseño de los elementos visuales, se tuvieron que incluir otros (como
los controladores) para que nuestro programa fuera capaz de comunicarse con los robots. En realidad los
programas externos se comunican con Player, y éste a su vez con el simulador Gazebo. Gazebo es un
simulador aún en proceso de desarrollo, por lo que algunas de las características que se le podría exigir a este
tipo de aplicación no se encuentran totalmente finalizadas o simplemente no existen. En concreto, aunque no
entra dentro de los intereses de este proyecto, no existen interfaces para establecer una comunicación con los
sensores sonar, por lo que se implementó una nueva basándonos en la ya existente para los infrarrojos.
Actualmente existen y se pueden utilizar varios tipos de controladores. En este proyecto se han
utilizado los siguientes:
a) differential_position2d: Utilizado para enviar órdenes de movimiento al robot y para obtener
información de la odometría, dada como la posición del robot en coordenadas globales.
b) sicklms200_láser: Esta interfaz se utiliza para obtener las lecturas de los sensores láser. Aunque el
modelo de láser “sicklms200” no se corresponde con el utilizado, servirá perfectamente para simular
el láser “hokuyo” de los robots.
También se desarrollaron varios entornos 3D donde poder realizar las pruebas. El más utilizado fue
una representación de las oficinas de Pal-Robotics (ver figura 3.3a), donde se intentó plasmar de forma fiel
cada una de las salas, incluyendo el mobiliario de éstas. Dentro de este entorno también se utilizaron algunas
zonas específicamente diseñadas para las pruebas (ver figura 3.3b), tal y como ocurre con los robots reales.
Un entorno de simulación bien desarrollado y fiel a la realidad, permitirá realizar simulaciones creíbles y así
4 (Object-Oriented Graphics Rendering Engine): librería utilizada para el desarrollo de gráficos y entornos en 3D.
obtener resultados parecidos a los conseguidos por los robots en un entorno real.
3.2 Software utilizado
En este apartado se explicarán las características relativas a los programas utilizados en este
proyecto, desde los aspectos a más bajo nivel, como el sistema operativo o los programas donde se realizó la
implementación, a otros como los simuladores o programas externos empleados, por ejemplo, para la
representación de datos de forma visual.
3.2.1 Sistema Operativo, C++ y entorno de programación
El sistema operativo utilizado fue Linux, en concreto la distribución Ubuntu 10.04 LTS. Ubuntu
[w13] está basado en la distribución Debian GNU/Linux y se distribuye de forma libre y en código abierto.
Pal-Robotics está actualmente adaptando Ubuntu 10.4 para ser el Sistema Operativo (en adelante SO)
principal de los robots (actualmente no lo es) ya que, a medida que las prestaciones hardware crecen, se
puede aprovechar este incremento de potencia para utilizar sistemas operativos más potentes. El objetivo
final es el de utilizar el mismo SO tanto en los ordenadores donde se desarrollan los códigos como en el
robot. Con esta medida se ahorra mucho tiempo en la creación, testeo y arreglo de errores que en dos
sistemas por separado (uno donde se desarrolla el código y otro el del robot).
Actualmente, como ya se ha comentado, Ubuntu es el SO utilizado para el desarrollo software, pero
otro SO es utilizado internamente por los robots: Oselas [w14]. Éste es un SO orientado a pequeños
sistemas/ordenadores empotrados. Se eligió porque ocupa poco tamaño, se carga en memoria RAM, puede
ser actualizado de forma automática y las aplicaciones se pueden desarrollar independientemente de cuál sea
la computadora del programador. Por contra, cada vez que se desarrolla una nueva aplicación para este SO,
ésta debe ser probada en el propio ordenador del desarrollador, y compilada y probada específicamente para
Oselas.
En cuanto al lenguaje de programación se decidió utilizar C++. Éste es un lenguaje de programación
multiparadigma, ya que como se explica en [b]: es procedimental, orientado a objetos, funcional, genérico y
con características de metaprogramación. Esta potencia y flexibilidad hace de C++ una herramienta sin
igual, pero que a su vez puede causar confusión: no se debe ver C++ como un lenguaje individual, sino
como un conjunto de lenguajes relacionados como C, C++ orientado a objetos, Template C++ y el STL.
Se decidió utilizar C++ en este proyecto por diversas razones. La primera de ellas es que este
proyecto se ha realizado dentro de la empresa Pal-Robotics y debía de amoldarse a su forma de trabajo, ya
que C++ es el lenguaje de programación allí utilizado prácticamente en su totalidad. Por otra parte, este
lenguaje es el más utilizado en el mundo de la robótica porque, además de que la mayoría de las librerías
existentes en este campo se encuentran implementadas en C++, muchas veces se necesitan realizar
ejecuciones rápidas o en tiempo real que con otros lenguajes sería imposible. A nivel personal también fue un
una ventaja, ya que aunque se tuvieron que revisar y ampliar algunos conceptos, principalmente de las
referencias [r] y [s], ya se había tenido la oportunidad de trabajar con este lenguaje de programación en
ocasiones anteriores.
A otras herramientas que también fueron utilizadas en la implementación de este proyecto hacemos
referencia a continuación:
a) CMake [w16]: Empleado para controlar el proceso de compilación de software. CMake utiliza
ficheros de configuración independientes de la plataforma y del compilador, generando “makefiles”
nativos y “workspaces” que pueden ser utilizados en nuestro entorno de compilación. Se utilizó entre
otras para añadir librerías y ficheros externos a nuestro proyecto, además de su uso necesario en la
utilización de las librerías propias de Pal-Robotics.
b) Valgrind [w15]: Herramienta que se utilizó para encontrar fallos en la implementación cuando se
producían errores en tiempo de ejecución. Principalmente se utiliza para “debugar” fallos en
memoria o para detectar fugas de memoria (“memory leaks”).
c) Kdevelop [w17]: Interfaz visual de programación utilizada para el desarrollo de la implementación.
Es libre, de código abierto para cualquier sistema operativo, y ofrece una gran variedad de “plugins”
que amplían sus características. Además de presentar el código de una forma agradable e intuitiva,
permite realizar operaciones de compilación y ejecución de forma sencilla.
3.2.2 Simuladores: Player, Stage y Gazebo
La utilización de simuladores en lugar de robots reales aportan diversas ventajas en el desarrollo de
este tipo de proyectos, tales como:
a) Rapidez a la hora de probar nuevos programas: Cuando se están realizando continuas
modificaciones en nuestros códigos, los simuladores permiten probar estos cambios de una forma
mucho más rápida que con los robots reales, ya que no es necesario compilar de nuevo (si el robot
posee un sistema operativo distinto al utilizado por el desarrollador) y enviar este código al robot
real.
b) Seguridad: Ejecutar implementaciones recientemente modificadas en un robot real puede resultar
peligroso, ya que un comportamiento no esperado podría causar daños físicos al robot, en el entorno
o a los propios usuarios, al contrario que en un simulador.
c) Compatibilidad entre las interfaces utilizadas en el simulador y por los robots reales: Cuando
un código se escribe de forma óptima, éste debe funcionar del mismo modo independientemente de
si se está ejecutando en un robot real o en un simulador. Esta capacidad debe de ser un elemento
transparente para el programador, permitiendo la creación de código sin tener en cuenta dónde va a
ser ejecutado.
d) Gran variedad de plataformas: Permiten probar nuevas implementaciones o modificaciones de
códigos en varios robots móviles que físicamente no se poseen.
Por otra parte, el uso de simuladores también posee un gran inconveniente:
• Una plataforma real no se comporta como su correspondiente modelo en simulación: Suele ser
común en robótica que al probar un programa en simulación el comportamiento o los resultados
obtenidos son distintos a los logrados con un robot real. Esto se debe a que es muy difícil crear un
modelo idéntico al original, además de que el simulador no puede recrear de forma exacta todas las
variables existentes en el mundo real (dinámica, por ejemplo).
En este proyecto se han utilizado una serie de herramientas que han permitido el testeo de la
implementación creada utilizando simuladores ya que en el campo de la robótica la utilización de éstos se
convierte en un elemento imprescindible en las fases de pruebas. Durante la realización se han utilizado las
herramientas de simulación desarrolladas por The Player Project [w1], creadores de software libre orientado
a la investigación en robótica y sensores. El paquete software utilizado está compuesto por Player, Stage y
Gazebo.
A. Player
Se trata de un servidor que ofrece una interfaz para interactuar con los sensores y actuadores de
robots sobre una red IP. Un programa, como el de calibración que se desarrollará en este proyecto, se
comunica con Player a través de un socket TCP, leyendo datos de los sensores y escribiendo comando a los
actuadores. En nuestro caso en particular, se le enviarán comandos a los actuadores motores para mover al
robot, y se leerán de los sensores encoders y del láser el desplazamiento de las ruedas (los ticks de los
encoders) y las lecturas del láser, respectivamente.
Player es una Capa de Abstracción del Hardware (Hardware Abstraction Layer, HAL) para
dispositivos robóticos, es decir, se establece una comunicación con simuladores como Stage y Gazebo o con
robots reales ofreciendo servicios al usuario, resultando transparente para el usuario la forma en la que
Player se comunica con los dispositivos. Este servidor ofrece una serie de interfaces con las que es posible
comunicarse con programas externos. En este proyecto se utilizará la interfaz position2d para interaccionar
con los motores y los encoders, y laser para la comunicación con el telémetro láser.
B. Stage
Este programa simula una población de robots móviles, sensores y objetos en un mapa de bits de dos
dimensiones. Está orientado para la investigación dentro de sistemas autónomos multi-agente, por lo que es
preferible su utilización con modelos simples y baratos (computacionalmente) antes que a intentar emular
dispositivos con gran fidelidad.
C. Gazebo
Se trata de un simulador multi-robot para entornos abiertos. Como Stage, éste es capaz de simular
poblaciones de robots, sensores y objetos, pero esta vez en un mundo tridimensional. Es capaz de
comunicarse de forma realista con los sensores y de interaccionar de forma convincente con los objetos del
entorno. Gazebo a su vez utiliza algunas librerías externas como ODE para la generación de las fuerzas
físicas (gravedad, por ejemplo) u OGRE para la visualización de gráficos en 3D.
3.2.3 Representación de resultados con Scilab
Scilab [w8] es un lenguaje de programación de código abierto ampliamente utilizado para la
realización de operaciones matemáticas. Puede ser utilizado para el procesamiento de señales, análisis
estadísticos, simulaciones u optimizaciones numéricas, entre otras. Es una alternativa a MATLAB,
compartiendo una sintaxis muy parecida y con la posibilidad de convertir un formato en otro fácilmente.
En este proyecto se ha utilizado para la representación de los resultados obtenidos por medio de
gráficas. Se escribió en este lenguaje un programa muy sencillo que permite mostrar de forma visual la
evolución de cada uno de los parámetros que se desean estimar con el paso del tiempo. Así, resulta más
sencilla la tarea del análisis de los resultados, ayudando a obtener conclusiones y permitiendo la inclusión de
estos datos en este documento en forma de gráficas.
3.2.4 Creación de diagramas con DIA
DIA [w10] es un software de código abierto utilizado para la creación de diagramas. Está inspirado
en el programa Visio (perteneciente a Microsoft) pero orientado a un uso más casual. Puede ser utilizado para
dibujar muchos tipos diferentes de diagramas, incluyendo los que resultan interesantes para este proyecto, los
diagramas UML.
Existe una gran variedad de diagramas pertenecientes al lenguaje UML. Nosotros nos centraremos
únicamente en dos:
a) Diagramas de clases: Describen la estructura de un programa mostrando sus clases, atributos y las
relaciones entre ellos. Los diagramas de clases son utilizados durante el proceso de análisis y diseño,
donde se muestra la información que se maneja y la relación que existe entre todos los componentes
del sistema.
b) Diagramas de actividad: Representan los flujos de las operaciones que se realizan en nuestro
programa. En otras palabras, son una representación de la algoritmia de un sistema.
3.3 Desarrollos previos
En este apartado se explicarán los formatos utilizados para la creación de logs, en qué consiste la
técnica de scan-matching utilizada, y por último qué librerías externas se utilizaron, junto a una breve
descripción de éstas.
3.3.1 Formatos para la creación de logs: JSON y CARMEN
Tanto la creación de logs para los datos odométricos, las lecturas del láser y los resultados devueltos
por el algoritmo de scan-matching, se realizarán siguiendo un formato previamente especificado y bien
conocido. De esta forma, la lectura de los datos podrá realizarse de una forma más ordenada y eficiente, a la
vez que se facilita la tarea de interpretación al usuario.
A. JSON
Para la realización de logs de la odometría, que se utilizarán para conocer las velocidades de las
ruedas en un intervalo, y del scan-matching, que permiten conocer la roto-traslación del robot, se utilizará el
formato JSON (JavaScript Object Notation) [w8]. Éste es un formato de intercambio de datos ligero, de fácil
escritura y lectura para los humanos, sencillo de analizar sintácticamente y de generar código por parte de las
máquinas. Basado en JavaScript, JSON es un formato de texto independiente de cualquier otro lenguaje,
pero que usa convenciones familiares para los programadores de C, C++, C#, Java, JavaScript, Perl, y
Python, entre otros.
JSON se compone de dos estructuras básicas:
a) Una colección de parejas utilizando el formato “nombre-valor”: En varios lenguajes de
programación a esto también se le llama objeto, estructura, diccionario, tabla hash, array asociativo,
etc.
b) Una lista ordenada de valores: En otros lenguajes de programación también llamado array, vector,
lista o secuencia.
A esta forma de estructurar los datos se le llama universal, ya que permite compartir información
entre distintos lenguajes de programación. Esto es posible gracias a que todos los lenguajes modernos
soportan este formato de una forma u otra.
La sintaxis del formato JSON es muy sencilla. El inicio y final de un objeto se delimita entre llaves
('{' y '}') y cada uno de estos está compuesto por una serie de parejas “nombre-valor”. El nombre de cada una
de las parejas está entrecomillado (“”) y seguido de dos puntos (':') se encuentra el valor al que hace
referencia. A su vez, cada una de estas parejas se encuentran separadas por una coma (',').
B. CARMEN
Para poder aplicar el algoritmo de scan-matcing utilizado en este trabajo es necesario crear
previamente un log con las lecturas del telémetro láser. Este log se escribirá en CARMEN [w9], un
formato muy sencillo pero suficiente para la utilización de Canonical Scan Matcher (CSM) [w4].
Este fichero log se compone de tantos objetos como lecturas de láser se hayan recibido. Cada uno de
estos objetos contiene una serie de campos que siguen el siguiente formato:
FLASER n_rayos [lecturas] x y z r p y timestamp_sec modelo_láser timestamp_usec
a) FLASER: etiqueta para indicar el inicio de una nueva lectura del láser.
b) n_rayos: indica el número de rayos guardados en cada una de las lecturas.
c) [lecturas]: este campo tendrá n_rayos valores, cada uno de ellos expresando el valor de cada uno de
los rayos en metros. Además, cualquier valor igual a cero será interpretado como un rayo inválido.
d) x y z r p y: indica los desplazamientos obtenidos por la odometría. En nuestro caso serán
establecidas a cero, ya que estos datos no serán utilizados.
e) timestamp_sec: los segundos del instante en el que se realizó la lectura.
f) modelo_láser: tipo de láser utilizado, en nuestro caso será “hokuyo”.
g) timestamp_usec: microsegundos del instante en el que se realizó la lectura.
3.3.2 Técnicas de scan-matching: PL-ICP
En muchas tareas de robótica móvil, tales como la planificación de trayectorias o construcción de
mapas, es necesario que el robot se encuentre correctamente localizado (debe conocer las coordenadas de su
posición x e y, y el ángulo de giro theta con respecto a un eje de coordenadas global). Una forma de
conseguir esto de modo barato y sencillo es mediante odometría, pero normalmente esta técnica resulta
insuficiente debido a la acumulación de errores que se producen con el paso del tiempo (explicado en el
apartado 2.1.4). Una posible solución a la corrección de errores provocados por la odometría consiste en la
utilización de técnicas de scan-matching, que permiten minimizar tales errores disponiendo únicamente de
un mapa, generado a partir de las observaciones de un sensor láser, y de medidas procedentes de la
odometría.
Una de las técnicas de scan-matching más populares en robótica móvil es Iterative Closest Point
(ICP). De forma iterativa éste método comprueba la traslación y rotación que se necesita aplicar para
minimizar la distancia entre dos nubes de puntos. Como entrada para la utilización del algoritmo se requieren
dos conjuntos de puntos, una estimación inicial de la transformación, y un criterio de parada de las
iteraciones. Como salida se obtiene una transformación, que ofrece la traslación y rotación realizada por el
robot móvil.
Estos algoritmos suelen seguir el siguiente orden en la ejecución:
a) Asociación de puntos siguiendo el criterio del vecino más cercano.
b) Estimación de los parámetros de la transformación utilizando una función de media cuadrática.
c) Transformar los puntos utilizando los parámetros estimados.
d) Iteraciones (re-asociación de puntos, etc.)
En este trabajo se va a utilizar el algoritmo de scan-matching PL-ICP presentado en [p]. Éste es una
variante de ICP que utiliza una métrica “punto-a-linea” en vez de “punto-a-punto”. Gracias a una forma de
minimización cerrada, esta técnica mejora a otras como MB-ICP, vanilla ICP e IDC en precisión, número de
iteraciones y velocidad de cálculo. Además, este método converge de forma cuadrática y en un número de
iteraciones finita.
En [w4] se presenta un ejemplo sobre la efectividad de este método. Se compara una ejecución de
scan-matching utilizando PL-ICP que tarda 7 iteraciones en converger (ver figura 3.2), mientras que la
misma situación utilizando el algoritmo Vanilla ICP toma 30 iteraciones. En la figura anterior los puntos
representan las medidas recogidas por los sensores (los azules tomados en el instante yt-1 y los rojos en el
siguiente instante yt), mientras que las líneas (las verdes son válidas mientras que las rojas inválidas) son los
errores que se pretenden minimizar.
Durante todo el desarrollo de este trabajo se utilizará este algoritmo, no para localizar al robot en
tareas de navegación o mapeado, sino para la obtención de la traslación y rotación del robot en un intervalo
de tiempo. La implementación de este algoritmo se encuentra desarrollada en un paquete bajo el nombre
CSM (Canonical Scan Matcher), tal y como se explica en el siguiente apartado.
3.3.3 Librerías externas: Eigen, CSM y JSON-Spirit
El programa de calibración automática creado utiliza librerías externas para así ampliar sus
posibilidades.
Estas librerías son:
a) Eigen: Se trata de una librería en C++ basada en templates para la realización de cálculos de álgebra
lineal: vectores, matrices y algoritmos relacionados. Entre sus características destaca su versatilidad,
rapidez, elegancia y su facilidad de compilación [w3]. La versión utilizada ha sido Eigen 2.0.14.
b) CSM: La librería Canonical Scan Matcher (CSM) [w4] es utilizada en este proyecto para la
aplicación de un algoritmo de scan-matching con el que se puede estimar la roto-traslación del
sensor láser, y por tanto del robot móvil. No se realizó un estudio intensivo de éste método aunque,
además de entender su funcionamiento, se tuvieron que realizar algunas modificaciones en el código
fuente, explicadas en capítulos posteriores.
c) Json-Spirit [w5]: Se trata de un parser (analizador sintáctico) escrito en C++ e implementado
utilizando Boost [w6]. JSON, ya explicado en apartados anteriores, es un formato de texto similar a
XML pero menos expresivo. Esta librería lee y escribe ficheros JSON o streams y soporta ASCII o
Unicode e implementaciones std::vector o std::map para crear objetos JSON. En este proyecto se
utiliza para la lectura de logs, facilitando enormemente la tarea de obtener cada uno de los
parámetros procedentes de una lectura de láser o de la odometría. Esta herramienta resultó de gran
utilidad al inicio del proyecto, cuando sólo se utilizaban logs para el tratamiento de datos.
3.3.4 Librerías pertenecientes a Pal-Robotics
El programa desarrollado utiliza librerías pertenecientes a Pal-Robotics, ofreciendo
implementaciones de algoritmos complejos y métodos que facilitan ciertas tareas. En este apartado no se
entrará en detalles por motivos de privacidad de la empresa, por lo que únicamente se realizará una
recopilación de los elementos incluidos en tales librerías:
a) Conectores: Permiten establecer una conexión entre nuestro programa con Player o con los robots
reales de forma sencilla. También existen conectores cuya finalidad es la de comunicarse con
distintos servicios, como algoritmos de planificación de trayectorias o evitación de obstáculos.
b) Sensores: Facilitan la creación de una interfaz entre cualquier tipo de sensor y nuestro programa,
permitiendo así la recepción de las lecturas de éstos.
c) Actuadores: Permite el envío de comandos de movimientos a los motores, en nuestro caso a las
ruedas, y por tanto conseguir el desplazamiento del robot.
d) Localización: Ofrece métodos para conocer la posición absoluta del robot, ya sea por medio de la
odometría o con la participación de sensores.
e) Navegación: Se calculan las trayectorias que debe de seguir un robot para llegar a un punto del
mapa. También se incluyen algoritmos de evitación de obstáculos como Vector Field Histogram
(VFH).
f) Ejecución multiproceso: Soporte para poder ejecutar hilos paralelos de ejecución.
g) Otros: Captura de parámetros en la ejecución del programa, utilización de distintos formatos para
mostrar mensajes por pantalla, muestreo de resultados de forma gráfica y escritura y lectura de
ficheros.
Capítulo 4: Calibración de Robots Móviles. Desarrollos de la investigación
En este capítulo se explicarán los desarrollos realizados en este trabajo final de Máster con el fin de
poder alcanzar los objetivos propuestos para el proyecto. Como es habitual en este tipo de proyectos, se
seguirá una metodología compuesta por tres etapas fundamentales: análisis, diseño e implementación. En
relación con el concepto de metodología que utilizaremos recogemos el punto de vista de varios autores ([aa]
y [bb]) que resaltan los diversos enfoques que caben en la investigación y la dificultad de distinguir el nivel
teórico y el práctico. En palabras de Asti Vera [aa], la metodología es el estudio analítico y crítico de los
métodos de investigación y prueba, donde sobre todo interesan los propios procesos de investigación.
4.1 Análisis de la problemática
La construcción física de un robot es una tarea que requiere una gran precisión, ya que de ello
dependerá el correcto funcionamiento de la plataforma. En robótica móvil, y en concreto en los robots que
nos interesan con direccionamiento diferencial, una calibración deficiente de sus parámetros físicos
provocará, entre otros, problemas en el cálculo de la odometría, repercutiendo en los programas que la
utilicen, como algoritmos de navegación o localización. También pueden surgir problemas similares con el
paso del tiempo, donde es común que los robots varíen sus configuraciones debido, por ejemplo, al desgaste
de las ruedas o a la pérdida de presión de los neumáticos.
También resulta de gran importancia la precisión con la que se calculan las posiciones de los
sensores. La gran mayoría de las técnicas utilizadas en robótica móvil, como por ejemplo aplicaciones de
mapeado o navegación, necesitan conocer las coordenadas reales de traslación (x,y y z) y rotación (roll, pitch
y yaw) de los sensores con respecto al eje de referencia del robot para poder realizar un tratamiento de la
información más detallado y realista.
Dado que normalmente no se tienen un alto número de robots por calibrar, y que ésta tarea no suele
realizarse con mucha frecuencia, es común realizar la calibración de los parámetros físicos del robot y de las
posiciones de los sensores de forma manual; un método rápido y sencillo pero que no resulta suficiente
cuando se requiere una alta precisión. Sin embargo, esta tarea también puede ser realizada por los propios
robots mediante diferentes técnicas; incluso se pueden aprovechar todos los sensores que posee para realizar
la calibración de estos junto a la de los parámetros físicos de forma simultánea. Es importante que este
proceso de calibración sea preciso y rápido, sobre todo cuando se desean calibrar un número elevado de
robots.
En los siguientes apartados se hará una breve introducción al estado del arte en algoritmos de
calibración en robótica móvil, recorriendo primeramente los métodos más representativos en la calibración
de la odometría de forma individual, para a continuación seguir con los orientados a la calibración de la
odometría y de los sensores de forma simultánea.
4.1.1 Calibración de los parámetros físicos del robot
Desde un punto de vista genérico, los métodos existentes para calibrar la odometría de un robot
móvil se pueden clasificar en: los basados en geometría o en la fusión de sensores. Otra posible distinción
consiste en la forma en la que la técnica de calibración interactúa con el movimiento del robot; son los
métodos de calibración “offline”, por ejemplo mediante la ejecución periódica de trayectorias fijas, o
técnicas que actualizan la odometría “online”, donde se aprovechan los datos de sensores exteroceptivos
(término explicado en el apartado 2.5.1).
A continuación se explicarán los dos tipos de métodos que contiene el primer tipo de clasificación.
A. Métodos de calibración basados en geometría
Referente en la calibración de robots móviles desde los años 90 y desde el cual han nacido métodos
más sofisticados, UMBmark (University of Michigan Benchmark) [i] es utilizado tanto para comparar la
precisión de la odometría en diferentes robots como para medir y ajustar los parámetros de un único robot.
Se basa en el método inicial “uni-directional square path” test, en el que el robot debe recorrer un
cuadrado de 4x4 metros empezando en una posición inicial x0,y0,θ0. Una vez finalizado, el robot debe acabar
en la posición inicial o en una posición cercana debido a los errores odométricos. Este método no es idóneo
para el cálculo de los errores odométricos, ya que podrían cancelarse distintos tipos de errores, por ejemplo
Eb (error de la distancia entre ruedas) y Ed (error de diámetro en las ruedas), y el robot podría finalizar su
recorrido en la posición inicial, dando la falsa sensación de que no se produjeron errores.
Para solucionar este problema se creó UMBmark, donde se recorrerá el cuadrado en ambas
direcciones de las agujar del reloj. Así, si en un sentido existen cancelaciones del error, será posible detectar
tal situación cuando se ejecute el algoritmo en el sentido contrario. Este método no sólo permite calcular los
errores odométricos, sino calcular los valores correctos de los radios de ambas ruedas y la distancia entre
éstas, consiguiendo así calibrar robots móviles y atenuar los errores producidos en la odometría.
En [v] se presenta otro método de calibración de los parámetros físicos, genérico para cualquier tipo
de base móvil, donde se utiliza no sólo el punto de finalización de la trayectoria del robot (como ocurre en
UMBmark), sino la forma del recorrido para calcular los parámetros del error y la matriz de covarianza.
Además, para la generación de la trayectoria son utilizados los grafos de Voronoi.
B. Métodos de calibración basados en la fusión de sensores
Al contrario que en el caso anterior, ahora se aprovecharán los sensores que posea el robot móvil
para, junto a los datos de los encoders, poder calibrar los parámetros físicos del robot de forma automática.
Un primer método se describe en [w], donde se propone la utilización de un giróscopo de fibra óptica
(OFG) combinado con la información suministrada por los encoders de las ruedas para mejorar la precisión
de la estimación de la posición. Se describe la formulación de un Filtro Extendido de Kalman (EKF)
utilizado para la combinación de los datos de los sensores anteriores.
Otro método que utiliza EKF es [y], donde se utiliza un giróscopo, junto a los encoders, y una unidad
GPS, para aplicar una propuesta de EKF y estimar la configuración de un robot en entornos exteriores.
En [m] se presenta un método que permite, de nuevo mediante EKF, la estimación simultánea de la
configuración del robot (parámetros físicos) y de los parámetros que caracterizan los errores sistemáticos
utilizando como entradas las lecturas de los encoders y de un telémetro láser. También permite, la estimación
de los parámetros no-sistemáticos, que en este caso es llevada a cabo por otro EKF donde las observaciones
son obtenidas por dos configuraciones del robot dadas por el EKF anterior.
En el documento [n], los autores usan un método de máxima similitud para estimar los parámetros
odométricos de un robot diferencial, utilizando las observaciones absolutas de una cámara externa. Éste
método resulta muy simple ya que el problema es completamente lineal, y es por ello que puede ser resuelto
por medio de mínimos cuadrados. Parte de la algoritmia presentada en [n] será aprovechada en este proyecto,
aunque en nuestro caso se considerarán mediciones relativas en vez de absolutas, e intervalos muy pequeños
en vez de trayectorias completas.
Otro tipo de algoritmo, descrito en [z], realiza las estimaciones utilizando mínimos cuadrados,
basadas en las ecuaciones cinemáticas del movimiento en un tiempo continuo, evitando así el error de la
discretización del tiempo. El uso de la técnica de mínimos cuadrados es posible cuando se trabaja en un
mapeado lineal, junto a las incógnitas y las mediciones, y no es el resultado de una linearización. La técnica
básica utiliza mediciones de cámaras y lecturas de la posición absolutas dadas por los encoders.
Por último, las medidas de varios sensores son explotadas en [x] donde, en contraste con otros
métodos que requieren mediciones explícitas del movimiento actual del robot cuando se está realizando la
calibración de la odometría, este algoritmo usa los sensores del robot para, de forma automática, realizar la
calibración mientras este realiza cualquier otra tarea. Un algoritmo incremental de máxima similitud permite
al robot adaptarse a los cambios en su odometría online justo en el momento en el que ocurren.
4.1.2 Calibración simultánea de la odometría y sensores
Otra fuente significativa de errores sistemáticos, cuando un robot móvil está equipado con más de un
sensor, proviene de la falta de precisión en el conocimiento de la transformación entre el eje de referencia del
robot y el de los sensores. En general, la calibración independiente de cada sensor resulta muy difícil y en
ocasiones imposible; además, la estimación a mano de los parámetros es muy pobre, por lo que las técnicas
de calibración simultánea de sensores se convierten en la mejor alternativa.
Un ejemplo de este tipo de procesos se observa en [ñ], donde se describe un método para la
calibración automática de un sistema multisensorial compuesto por un par de encoders ópticos
incrementales, un giróscopo y una brújula magnética. En éste se explota la redundancia de las lecturas de los
sensores para identificar los parámetros previamente desconocidos: el radio de las ruedas Rr, Rl y la distancia
B entre éstas para la odometría, el factor de escala Kgyro para el giróscopo y los cinco parámetros de la elipse
para la brújula, a, b, mx, my y α. La redundancia a la que nos referimos es la información sobre de la
orientación del robot, que es ofrecida por los tres tipos de sensores.
En el documento [u] se describe una estrategia para calibrar, de de forma simultánea y extrínseca, un
sensor de distancia (por ejemplo telémetro láser) montado en un vehículo y los parámetros físicos de éste.
Esta estrategia consiste en mover al robot por una serie de caminos circulares en un entorno que contenga
una pared fácilmente detectable por el sensor láser.
4.2 Diseño del sistema
En este apartado se estudiará en profundidad el documento [a], utilizado en este trabajo de
investigación y que explica el algoritmo de calibración automática y simultánea de la odometría y la posición
de un sensor de un robot móvil. También se detallarán los métodos utilizados para la recogida de datos, su
procesamiento y la forma en que son utilizados por el algoritmo. Además, se explicarán las configuraciones y
modos de funcionamiento del sistema desarrollado, con los que se obtendrán distintos comportamientos y
resultados.
4.2.1 Estudio del documento “Simultaneous maximum-likelihood calibration of odometry and sensor parameters”
A continuación se realizará el estudio del documento [a], donde se explica el algoritmo de
calibración que se utilizará en este trabajo. En tal proceso se pretende calibrar la odometría y los parámetros
de la posición de un telémetro láser de un robot móvil, proceso al que llamaremos a partir de ahora
algoritmo de calibración. Además este método no requiere utilizar sensores o dispositivos externos ni que
el robot lleve una determinada trayectoria. Para la odometría se considerarán tres parámetros propios de los
robots móviles con direccionamiento diferencial, los radios de las ruedas y la distancia entre éstas (rL, rR y b),
y para el sensor láser otros tres parámetros de posición con respecto al eje de coordenadas del robot (l = (lx,
ly, lθ )). Se supondrá que el láser del robot está montado horizontalmente, ya que en otro caso se necesitarían
otros dos parámetros para las rotaciones “roll” y “pitch”. También se asume que son conocidas tanto las
medidas de las velocidades de las ruedas como las lecturas ofrecidos por el sensor láser, datos que se
explicará en apartados posteriores cómo son recogidos y tratados.
A partir de este punto se examinarán por separado cada una de las partes del documento: modelo
cinemático, estimación de los parámetros J21 y J22, estimación de los otros parámetros b, lx, ly, lθ y por último
la resolución del problema de mínimos cuadrados con restricciones.
Consideraremos el movimiento de un robot a lo largo de una trayectoria arbitraria q(t). Además, el
eje del tiempo se dividirá en una serie de intervalos, cada uno delimitado por dos mediciones consecutivas
del sensor láser y de duración Tk (los intervalos pueden ser de duración variable). En el intervalo k-th, el
robot se mueve desde la posición qk a la posición qk+1, realizando una roto-traslación
ok≙qk 1⊖qk
Dicha roto-traslación depende de las velocidades de las ruedas ωL(t), ωR(t) a través de los parámetros
rL, rR y b. Así, en un primer instante, ok es un valor desconocido. En el mismo intervalo de tiempo, el
movimiento del sensor es sk en el eje de coordenadas absoluto:
sk≙qk1⊕l ⊖qk⊕l
Utilizando un algoritmo de scan-matching se puede obtener una estimación ŝk de sk
A partir de este punto, por simplicidad, se asumirá que las velocidades de las ruedas en cada uno de
los intervalos es constante, permitiendo la utilización de fórmulas más simples, una implementación eficiente
y una buena precisión en la configuración utilizada para las pruebas.
Estudio del Modelo cinemático
Se considera el modelo cinemático del uniciclo
ddt [qx t
qy t qθ t
]=[ v t cosqθt v t sin qθt
ωt ] (4.1)
Las velocidades absolutas v(t), ω(t) dependen de las velocidades de las ruedas a través de la
transformación lineal J:
[ v t ωt ]=J [ω Lt
ωR t ]Tal transformación depende de los parámetros odométricos
J =[ J 11 J 12
J 21 J 22]=[r L/ 2 r R /2
−r L/b r R /b] (4.2)
En este documento, siempre se considerará cada intervalo por separado. Así, se fijará t=0 al inicio
del intervalo y la posición inicial será qk =q(0)=0. Si se asume que las velocidades de las ruedas son
constantes durante todo el intervalo (ωL(t) = ωLK, ωR(t) = ωR
K), entonces las velocidades del robot son
constantes también:
v(t) = J11 ωLk+ J12ωR
k = V0k (4.3)
ω(t) = J21 ωLk + J22ωR
k = ω0k (4.4)
De esta forma se puede calcular la roto-traslación al final del intervalo qk+1 = q(Tk) = ok de forma
cerrada integrando la ecuación diferencial (4.1)
oxk = v0
k Tk (sin ω0kTk) / (ω0
kTk) (4.5)
oyk = v0
k Tk (1 – cos ω0kTk) / (ω0
kTk) (4.6)
oθk = ω0kTk (4.7)
Estimación de los parámetros J21 y J22
En primer lugar se estimará J21 y J22 utilizando la estimación de la rotación devuelta por el algoritmo
de scan-matching. Esta parte del método es numéricamente equivalente a la estimación realizada en [n]. Las
principales diferencias con nuestro caso estriban en que se están considerando medidas relativas en vez de
absolutas y la utilización de intervalos muy pequeños en vez de trayectorias completas.
Si el robot realiza una rotación de un ángulo oθk, también el sensor rotará el mismo ángulo: sθk = oθ
k.
Así, utilizando la estimación del algoritmo de scan-matching ŝk se puede observar que oθk = ω0kTk. De las
fórmulas (4.4) y (4.7) se puede deducir:
(J21 ωL + J22ωR)Tk = sθ
k
Ahora se reescribirá la fórmula anterior en forma matricial con las mediciones reales (procedentes de
los sensores del robot), donde ŝk será la estimación de sθk dada por el algoritmo de scan-matching, y ŵL, ŵR las
medidas de las velocidades de las ruedas:
[ ŵLk T k ŵR
k T k ] [ J 21
J 22]=ŝθ
kerrores (4.8)
Por simplicidad del texto, no se escribirán lo detalles de “errores”. En cuanto a ŵLkTk y ŵR
kTk cabe
comentar que son simplemente los incrementos de los encoders de los motores en el intervalo k-th y su error
se traduce en un pulso como mucho. Por otra parte, la varianza de ŝθk puede tenerse en cuenta al resolver el
problema de mínimos cuadrados, dependiendo de si el algoritmo de scan-matching lo permite o no.
Si adicionamos todas las ecuaciones para cada intervalo k se obtiene:
[. .. .. .
ŵLk T k ŵR
k T k
. .
. .
. .][ J 21
J 22]=[
.
.
.ŝθ
k
.
.
.]errores
Con todo esto, se puede encontrar una estimación para ĵ21 y ĵ2 2 de forma lineal utilizando mínimos
cuadrados.
Cabe destacar, en este punto, que sólo se conoce una estimación de J21 = -rL/b y J22=+rR/b, pero
siguen siendo una incógnita los parámetros físicos rL, rR y b. De aquí en adelante se estimarán los cuatro
parámetros b, lx,ly,lθ y así, la estimación de rL y rR será dada por r L=−b J 21 y r R=b J 22
Estimación de los otros parámetros b, lx,ly y lθ
Observando las características geométricas de la figura 4.1, se puede obtener la siguiente restricción
para los valores de l, ok y sk:
l⊕ sk=ok⊕l (4.9)
Reescrito de forma extendida para las componentes x e y se obtiene:
l xs xk cos lθ – s y
k sin lθ=oxkl x cosoθ
k – l y sin oθk (4.10)
l ysxk sin lθs y
k cos lθ=oyk l x sin oθ
k – l y cos oθk (4.11)
En este punto ya se tiene una estimación de oθk (dado 4.7) y la estimación de las velocidades de las
ruedas en el intervalo k-th), por lo que también se tiene una estimación para los términos cos(oθk) y sin(oθ
k).
Ahora se le otorgará a oxk y oy
k una expresión que dependa del otro parámetro b. Así, las relaciones (4.10)-
(4.11) contendrán los cuatro parámetros restantes b, lx,ly y lθ. De la ecuación (4.2) se puede observar que J11 y
J12 pueden ser expresadas de forma que dependan de J21, J22 y b:
J 11=−b2
J 21 J 12=b2
J 22
Así, basándonos en las ecuaciones anteriores y (4.3), la velocidad lineal v0k puede ser estimada
como:
v0k=−b
2J 21 ωL
kb2
J 22ωRk (4.12)
Sustituyendo (4.12) dentro de (4.5)-(4.6), encontramos que oxk y oy
k son proporcionales a b utilizando
dos constantes cx y cy:
oxk = cxb oy
k = cyb (4.13)
Las constantes cx y cy dependen de valores que ya han sido calculados:
c x=12
T k −J 21ωLk J 22 ωR
k sin ω0
k T k
ω0k T k
c y=12
T k −J 21 ωLk J 22ωR
k 1−cosω0
k T k
ω0k T k
Se sustituye (4.13) en (4.10), y (4.11) para obtener la siguiente relación, que contiene los cuatro
parámetros aún desconocidos:
l xs xk cos lθs y
k cos l θ=cy bl x sin oθk−l y cosoθ
k
(4.14)
l ysxk sin lθsy
k cos l θ=c y bl x sin oθkl y cos oθ
k
(4.15)
Se define un vector x con los parámetros desconocidos:
x=[b , l x , l y , cos l θ ,sin lθ ]T
Tratamos cos lθ y sin lθ como dos variables separadas x4 y x5, con la restricción adicional x42 +x5
2 = 1.
Y reescribimos (4.14)-(4.15) en forma matricial Lk x = 0:
[−cx 1−cosôθk sin ôθ
k ŝ xk −ŝ y
k
−cy −sin ôθk 1−cosôθ
k ŝ yk ŝ x
k ] x=[00] (4.16)
La matriz Lk depende de valores conocidos. Definiendo M =k LkT LK , el problema de mínimos
cuadrados a resolver es:
min xTMx (4.17)
sujeto a la restricción x24 +x2
5 = 1 (4.18)
Para este problema, para cada solución x , también −x es una solución. Como restricción
adicional, se impone que la distancia b debe ser positiva:
x1 > 0 (4.19)
Resolución del problema de mínimos cuadrados con restricciones
Tendremos ahora que resolver el problema de mínimos cuadrados usando los multiplicadores de
Lagrange. La restricción (4.18) está escrita en forma matricial como
xTWx = 1, con W ≙[O3x3 O3x2
O2x3 I 2x2] (4.20)
La restricción (4.19) será ignorada por el momento, ahora simplemente se intercambiará la solución
si x resulta ser negativa.
Utilizando el método de los multiplicadores de Lagrange, una condición necesaria para una buena
optimización es que
(M + λW) x = 0
Se necesita encontrar un valor de λ tal que la matriz (M + λW) sea singular (matriz con determinante
cero), y después elegir la solución x en el núcleo de dicha matriz. El valor de λ puede ser encontrado
resolviendo la ecuación
det (M + λW) = 0
En nuestro problema, la matriz M tiene una estructura particular, con algunos ceros y elementos
repetidos:
M =[M 11 O m13 m14 m15
. m22 0 m35 −m34
. . m22 m34 m35
. . . m44 0
. . . . m44
]Para esta matriz, el determinante det (M + λW) es un polinomio de segundo grado (aλ2 + bλ +c)
donde los valores de a, b y c son como siguen:
a = m11*m222 − m22*m13
2 (4.21)
b = 2*m13*m22*m35*m15 − m222*m15
2−2*m11*m22*m352 +2*m13*m22*m34*m14 − 2*m22*m13
2*m44 − m222*m14
2 +
2*m11*m222*m44 + m13
2*m352 − 2*m11*m22*m34
2 + m132*m34
2
c = -2*m13*m353*m15 − m22*m13
2*m442 + m11*m22
2*m442 + m13
2*m352*m44 + 2*m13*m22*m34*m14m44 +
m132*m34
2*m44 − 2*m11*m22*m342*m44 − 2*m13*m34
3*m14 − 2*m11*m22*m352*m44 + 2*m11*m35
2*m342 +
m22*m142*m35
2 − 2*m13*m352*m34*m14 − 2*m13*m34
2m35*m15 + m11*m344 + m22*m15
2*m342 + m22*m35
2*m152 +
m11*m354 − m22
2*m142*m44 + 2*m13*m22*m35*m15*m44 + m22*m34
2*m142 − m22
2*m152*m44
El número λ , tal que la matriz M λ W sea singular, puede ser encontrado de forma cerrada
usando la bien conocida fórmula de las raíces de los polinomios de segundo grado. Después de haber
encontrado λ , la elección de x es única dada las restricciones (4.18) y (4.19). Sea υ cualquier vector en el
núcleo de M λ W . Para obtener la estimación de x , hay que escalar υ utilizando υ42υ5
2 con
respecto a la restricción (18), luego se intercambiará por el signo de υ1 con respecto a la restricción (4.19):
x=sign υ1
υ42υ5
2υ
Finalmente, los seis parámetros han sido encontrados:
b= x1
r L= x1∗ J 21
r R=− x1∗ J 22
l x= x2
l y= x3
l =atan2 x5 , x4
4.2.2 Cálculo y almacenamiento de los datos de la odometría y del láser
Un proceso fundamental y necesario para la utilización del algoritmo de calibración es la recogida de
datos. Son necesarios tanto los procedentes de la odometría como los del sensor láser, y tanto la calidad
como la cantidad de éstos resulta crucial para obtener resultados precisos. Además, estos datos deben
almacenarse con un formato específico para que su posterior lectura pueda realizarse de forma sencilla y
sobre todo rápida. A continuación se detallará las características de este proceso para cada uno de los tipos de
datos que se utilizarán.
A. Lecturas de los encoders y la odometría
El algoritmo de calibración, como se explicará en apartados posteriores, necesitará conocer las
velocidades angulares de las ruedas del robot en una serie de intervalos. Tales velocidades podrán ser
calculadas de dos formas distintas:
a) A partir de la información recibida de los encoders: Los encoders de los motores ofrecen los ticks
(pulsos), de forma incremental, en el instante en que son pedidos. Así, utilizando dos de estas
medidas tomadas en instantes consecutivos, es posible conocer la diferencia de ticks para cada una
de las ruedas y así estimar las velocidades angulares de éstas.
b) Utilizando los datos de la odometría: Otro método para conocer las velocidades angulares es
mediante odometría. Esta técnica devuelve las posiciones globales del robot en instantes
consecutivos y, como en el caso anterior, utilizando dos de estas medidas es posible conocer las
velocidades de las ruedas.
A continuación se detallarán cada una de las técnicas anteriormente propuestas.
Cálculo de las velocidades angulares de las ruedas utilizando la información de los
encoders
Tal y como se explica en [t], para un tramo de distancia recorrido durante un diferencial de muestreo
del encoders, se tiene:
ΔU =2 rN
ΔN 1,2(4.22)
donde ΔU es la distancia recorrida por una rueda, N el número de pulsos en una revolución completa de la
rueda y N1,2 el número de pulsos que captaría el encoders cuando la rueda se desplaza una distancia ΔU.
Así, se podrán calcular las velocidades de las ruedas como
ωi=ΔU i
t i (4.23)
ωd=ΔU d
t i (4.24)
en el cual ΔUd/i es el desplazamiento en radianes de la rueda derecha/izquierda y ti el tiempo empleado en
realizar ese giro.
Cálculo de las velocidades de las ruedas mediante odometría
Mediante la ecuación (4.25), ya propuesta en el apartado 2.3.1, es posible calcular el desplazamiento
de un robot dada la velocidad lineal y de rotación en un instante de tiempo como
[ x t y t t ]=[cost 0
sint 00 1][ υ t
ω t ] (4.25)
donde se tiene que
x t =cosθ∗υ t
(4.26)
y t=sin θ∗υ t
(4.27)
θ t =ωt
(4.28)
y por tanto, despejando υ y ω, se puede obtener la velocidad lineal y de rotación del robot como
υ t= x t /cosθ
(4.29)
υ t= y t / sinθ
(4.30)
ω t=θ t
(4.31)
Dadas además las ecuaciones de las velocidades angulares de las ruedas, citadas también en el
apartado 2.3.1
ωi=υ−b/ 2∗ω
r (4.32)
ωd=υb/2∗ω
r (4.33)
y sustituyendo las ecuaciones (4.29)-(4.31) en las anteriores, es posible calcular las velocidades angulares de
las ruedas como
ωi=bω2υ
2r (4.34)
ωd=−bω2υ
2r (4.35)
donde r es el radio de ambas ruedas y b la distancia entre éstas.
Independientemente del método que se utilice para calcular las velocidades de las ruedas, los datos
requeridos para tales cálculos deberán ser previamente recogidos y almacenados para su posterior uso. Esto
podrá realizarse de dos formas distintas: guardándolos en un fichero log o directamente en la memoria del
programa. Tales métodos serán detallados a continuación.
Almacenamiento de las lecturas en un fichero log
Una de las formas de acceder a los datos requeridos para realizar el cálculo de las velocidades de las
ruedas es mediante ficheros log. El formato utilizado, tanto para las lecturas como para las escrituras, será
JSON, ya explicado en el apartado 3.3.1A.
Tabla 4.1:Muestra de un objeto perteneciente a un log de odometría en formato JSON{ "timestamp": [0,464514], "left": -201, "right": 292, "odometry":
[0.00166974, 2.37345e-12,0.00150512] }
En la tabla 4.1 se muestra un ejemplo de un objeto de un log de odometría. A continuación se
muestra la composición de un elemento de tal log y los valores de cada parejas nombre-valor:
a) La primera ser refiere al timestamp (instante de tiempo) en el que fueron recogidos los datos, donde
el valor se compone de un array con dos elementos, los segundos con un valor de 0 y microsegundos
con un valor de 464514.
b) La segunda pareja pertenece al valor incremental del encoder izquierdo, que tiene un valor de -201
pulsos.
c) La tercera pertenece al encoder derecho, con un valor de 292 pulsos.
d) Por último, los coordenadas globales devueltas por la odometría y representada con un array de tres
números en coma flotante: el desplazamiento con las coordenadas x e y, 0.00166974 y 2.37345e-12
metros respectivamente, y la rotación con el ángulo theta, con un valor de 0.00150512 radianes.
Almacenamiento de las lecturas directamente en memoria
En este método los datos de la odometría no se guardarán en un fichero log, sino que lo harán
directamente en la memoria del programa. Con esto se consigue acceder a los datos de una forma mucho más
rápida y una implementación más “limpia”, evitando la creación de fichero externos al programa.
Una vez almacenados los datos de la odometría o de los encoders, éstos ya podrán ser utilizados para
estimar las velocidades de las ruedas en un intervalo (las velocidades en cada uno de los intervalos se
supondrán constantes para realizar los cálculos de una forma más sencilla y rápida). Durante todo el
desarrollo de este trabajo sólo se utilizarán los datos de la odometría, ya que ni los robots reales ofrecen la
posibilidad de acceder a la información de los encoders (por motivos externos al autor) ni el simulador
Gazebo/Player es capaz de emular tales sensores. Así, los datos procedentes de la odometría, a partir de
ahora los datos de odometría, serán los únicos utilizados para el cálculo de las velocidades de las ruedas.
Aún con todo, los datos de los encoders serán tenidos en cuenta en la creación de logs e incluso se
implementará un método para el cálculo de las velocidades de las ruedas a partir de éstos para posibles
trabajos futuros.
B. Lecturas del sensor láser
Otro tipo de dato que se utilizará, además de las velocidades angulares de las ruedas, son las lecturas
procedentes de un sensor láser, a partir de ahora los datos del láser. Estos, como se verá en apartados
posteriores, serán utilizados por un algoritmo de scan-matching con el fin de conocer la roto-traslación del
robot.
Como en el caso del apartado anterior con los datos de odometría, existen dos formas de almacenar
las lecturas recibidas por el láser, que revisaremos a continuación:
a) Almacenamiento de las lecturas del láser en un fichero log: Las lecturas serán almacenadas en un
fichero log en formato CARMEN, ya explicado en el apartado 3.3.1B. En este formato cada lectura
será un elemento compuesto por el número de rayos utilizados, los valores de cada uno de estos
rayos, el modelo del láser y el instante de tiempo en el que fueron registrados.
Tabla 4.2:Muestra de un objeto perteneciente a un log de láser en formato CARMENFLASER 181 4.42167 4.42247 4.42383 ... 1.49274 1.49156 1.49099
0 0 0 0 0 0 134 hokuyo 317743
En la tabla 4.2 se muestra un ejemplo de un elemento del log del láser en formato CARMEN. En esta
configuración se utiliza un modelo de láser hokuyo, se recogen 181 rayos con sus correspondientes
valores, y el instante en el que fueron recogidos, en este caso en el segundo 134 y microsegundo
317743.
b) Almacenamiento de las lecturas del láser directamente en memoria: En este método los datos
del láser no serán almacenados, sino que serán tratados directamente por un método de scan-
matching. Una vez obtenidos los resultados de éste las lecturas del láser no volverán a ser utilizadas,
por lo que carece de sentido su almacenamiento. Con esta medida se evita sobrecargar la memoria
del sistema con datos que no se volverán a utilizar, además de que el tiempo que el sistema
permanece ocioso esperando nuevos datos de láser puede ser aprovechado para realizar tareas como
ésta, adelantando así trabajo que igualmente habría que realizar más adelante.
Según lo explicado puede parecer contradictorio almacenar en una primera opción las lecturas
originales del láser, y en la otra descartarlas y almacenar únicamente los resultados obtenidos por el
algoritmo de scan-matching. Esto se hizo así para que los logs almacenaran la información más
primitiva (o básica) y poder de esta forma reproducir un experimento completo desde el inicio.
4.2.3 Preprocesado de los datos
Una vez obtenidos y guardados los datos de odometría y del láser, éstos deberán ser tratados por
distintas técnicas antes de poder pasar al siguiente paso, la fusión de datos. Las técnicas que se van a utilizar,
explicadas a continuación, son: un algoritmo de scan-matching a partir de las lecturas del láser y una
interpolación de los datos de odometría.
A. Aplicación del algoritmo de scan-matching CSM
Asumiendo que la roto-traslación de un telémetro láser acoplado a un robot móvil es equivalente a la
roto-traslación del robot en un intervalo de tiempo, y utilizando dos lecturas de láser consecutivas, se puede
aplicar el algoritmo de scan-matching CSM (explicado en el apartado 3.3.2) y así obtener los parámetros
que definen la roto-traslación del robot. A partir de este punto a los datos devueltos por el algoritmo de scan-
matching se les llamará “datos de CSM”. CSM puede ser utilizado de dos formas distintas dependiendo de
la fuente de procedencia de las lecturas del láser, tal y como se explica a continuación:
a) CSM recogiendo datos de un fichero log: En éste caso se tiene a disposición un fichero log con
todas las lecturas del sensor láser almacenadas de forma consecutiva. Tal y como se observa en la
figura 4.2a, se procederá del siguiente modo:
1. Primeramente se leerán los datos de forma iterativa de un fichero log en formato CARMEN.
2. Para cada una de las lecturas se descartarán aquellas lo suficientemente parecidas a la
anterior.
3. También se descartarán aquellas que no cumplan el formato CARMEN correctamente.
4. A los parámetros de odometría asociados a cada lectura láser se les añadirá una estimación
de ruido.
5. A cada pareja consecutiva de lecturas láser se les aplicará CSM.
6. Por último, se guardarán los resultados procedentes de CSM en otro fichero log en formato
JSON.
Tabla 4.3:Ejemplo de un elemento de un log de CSM{"valid":1, "x":[1.530755e-02,9.128221e-04,5.422770e04],
"iterations":4, "nvalid":169, "error":2.363662e-01, "láser_ref_timestamp":[1,384943], "láser_sens_timestamp":
[1,456346]}
En la tabla 4.3 se expone un elemento del log de resultados utilizando JSON, cuya sintaxis ya fue
explicada en el apartado 3.3.1A. Cada uno de los elementos de este log se compone de las siguientes parejas
nombre-valor:
1. La primera informa de la validez de los datos, siendo un valor 0 inválido y valor 1 válido. En
este caso el elemento es válido con un valor 1.
2. La segunda es un array de tres elementos con los valores de la roto-traslación del láser,
siendo los dos primero valores el desplazamiento representado con las coordenada x e y, con
valores 1.530755e-02 y 9.128221e-04 metros respectivamente, y la rotación con el ángulo
theta, con un valor de 5.422770e04 radianes.
3. La tercera es el número de iteraciones realizadas por el algoritmo, con un valor de 4.
4. La cuarta el número de rayos válidos, en este caso 169.
5. La quinta, el error cometido durante el proceso y con un valor de 2.363662e-01.
6. La sexta el timestamp de la lectura de láser referencia, con un valor de 1 segundo y 384943
microsegundos.
7. Por último, la séptima, es el timestamp de la lectura de láser siguiente, con un valor de 1
segundo y 456346 microsegundos.
b) CSM tratando las lecturas procedentes directamente del sensor láser: En este caso no se
dispondrá de todas las lecturas láser como ocurría en el anterior, sino que se irán utilizando las
medidas directamente conforme son leídas por el telémetro láser. Por otra parte, aunque en este caso
las lecturas serán del tipo cadena de caracteres, se mantendrá el mismo formato CARMEN que en el
caso anterior. Seguidamente se explicará el proceso igual que en el caso anterior:
1. En primer lugar se leerá la lectura recién capturada por el sensor láser.
2. Si esta medida es lo suficientemente parecida a la anterior, será descartada.
3. También será descartada si no cumple el formato CARMEN correctamente.
4. A los parámetros de odometría asociados a la lectura láser se les añadirá una estimación de
ruido.
5. Se aplicará CSM a la lectura láser actual y a la anterior, que fue guardada en la iteración
previa.
6. La lectura actual será guardada para que en la siguiente iteración sea utilizada como lectura
anterior.
7. Por último, se guardará el resultado procedente de CSM en la memoria del programa.
Como se explica en el apartado 4.2.3, la eliminación de lecturas muy parecidas resulta muy
importante, ya que los algoritmos de scan-matching no se suelen comportar bien en esta situación. También
resulta de interés la adición de ruido a los parámetros de odometría asociados a las lecturas basándose en la
desviación típica de ruido gaussiano, ya que el robot puede sufrir errores odométricos no-sistemáticos, es
decir, los producidos por factores como el deslizamiento de las ruedas. Añadiendo tal ruido se consigue que
los desplazamientos y rotaciones estimados por el scan-matching sean más parecidos a los estimados por la
odometría.
Una de las características a resaltar de CSM es la posibilidad de variar los parámetros de
configuración, ampliando así las posibilidades a la hora de aplicar el algoritmo. Una configuración adecuada
dependerá de factores como las características de la plataforma móvil a utilizar, el tipo de terreno por donde
se desplazará el robot o las propiedades del sensor láser.
Algunos de los parámetros modificables más interesantes son los siguientes:
• La diferencia mínima épsilon entre dos rayos, descartando aquellas lecturas que tengan todos sus
rayos con una diferencia menor que tal valor, es decir, aquellas que sean muy parecidas.
• Los valores utilizados para añadir ruido a los parámetros de odometría asociados a una lectura láser
aplicando la desviación típica de ruido gaussiano, donde se utiliza un valor sigma para el error del
ángulo theta y otro valor sigma para el error las coordenadas x e y.
• El tipo de algoritmo de scan-matching: (pl)ICP, gpm-stripped o HSM.
• El rango máximo y mínimo permitido para cada uno de los rayos del telémetro láser.
• Número máximo de iteraciones que podrá realizar el algoritmo de scan-matching.
B. Interpolación de los datos de odometría
En lo explicado hasta este punto, tal y como se mencionó en el apartado 4.2.2A, tenemos una serie
de datos de odometría cuyos timestamps corresponden con los instantes de tiempo en los que fueron
recogidos. Como se verá en el apartado siguiente, es necesario conocer los datos de odometría justo en el
instante en el que los datos del láser fueron recogidos, es decir, para cada lectura láser se necesita una lectura
de odometría cuyos timestamps coincidan. Ya que resulta muy complicado que estos valores coincidan de
forma exacta, se utilizará la interpolación de datos para solventar este problema.
La interpolación lineal [q] es un proceso relativamente sencillo. Tal y como se observa en la figura
4.3, se usa para hallar un valor aproximado de una función en un punto x de un intervalo de x 0 a x1. La recta
y = p1(x) = a0 + a1x que pasa por los puntos (x0,y0) y (x1,y1) cumple las dos ecuaciones lineales
y0=a0a1 x0 , y1=a0a1 x1 (4.36)
con solución
a0=y0 x1− y1 x0
x1− x0
, a1=y1− y0
x1− x0
(4.37)
Junto con (4.36) y (4.37) se obtiene
y= y0 x1− y1 x0
x1−x0 y1− y0
x1−x0 x (4.38)
que puede simplificarse como
y= y0x−x0 y1− y0
x1−x0 (4.39)
A continuación se expondrá un ejemplo para explicar el problema al que nos enfrentamos. Tal y
como se observa en la figura 4.3, suponemos una captura del proceso de recogida de datos entre los segundos
2 y 3. Las líneas azules indican el instante en el que se realizó una lectura del láser, mientras que las rojas
muestras cuándo se hizo lo propio con las de odometría. Supongamos que R1 y R2 son dos lecturas de
odometría consecutivas, y L una lectura de láser, encontrándose esta última entre R1 y R2. Como se comentó
anteriormente, se necesitan conocer los datos odométricos en el instante de cada lectura del láser; en el
ejemplo, para la lectura L, como no existe ninguna medida de odometría en ese instante (2.5 seg.), se deberá
de aplicar una interpolación de los datos odométricos de R1 y R2 para estimar los valores correspondientes
en el instante L.
Ahora se procederá a explicar la aplicación de este método en nuestro entorno de trabajo con la
ayuda del diagrama de actividad de la figura 4.4. En primer lugar, y para cada una de las lecturas de láser L,
se buscarán las dos lecturas de odometría R1 y R2 (anterior y posterior) más cercanas a ésta. A continuación
se calculará el peso que tiene el instante de la lectura láser con respecto al de los datos de odometría, siendo
tal peso de mayor tamaño cuanto más cerca se encuentra la lectura láser de la lectura de odometría con un
timestamp mayor (R2), y viceversa. Ahora, gracias a este factor de peso, se pueden estimar los valores de la
odometría en el mismo instante que en el de la lectura láser.
En el ejemplo de la figura 4.4, el peso de la lectura de láser L sería un valor proporcional entre la
distancia de ésta y R1 y R2 (supongamos 0 si L coincide con R1 y 1 con R2), y ya que L se encuentra justo
entre los valores de odometría, los nuevos datos odométricos interpolados coincidirán con la media
aritmética de los datos de R1 y R2.
Tal y como ya se ha visto en otros casos anteriores, la forma de almacenar los datos será distinta
dependiendo de la forma en la que se tratan los datos:
Datos interpolados almacenados en un fichero log: Los nuevos datos de odometría interpolados
serán almacenados en un fichero log.
Datos interpolados almacenados en la memoria del programa: Al contrario que en el caso
anterior, los nuevos datos interpolados serán almacenados directamente en la memoria del programa
y sin la necesidad de crear ficheros externos.
4.2.4 Fusión de los datos de odometría y CSM
En el apartado anterior se explicó cómo aplicar el algoritmo CSM e interpolación de los datos a las
lecturas previamente almacenadas del láser y de la odometría para crear unos nuevos. Es ahora cuando se
utilizarán estos nuevos datos para realizar una combinación con ellos y poder ser utilizados por el algoritmo
de calibración explicado en el apartado 4.2.1.
En primer lugar se dividirá la línea temporal de toda la ejecución en una serie de intervalos,
intervalos finales de aquí en adelante, asociándoles unos nuevos datos creados a partir de la combinación de
los de CSM y los odométricos interpolados. El tamaño de estos intervalos está delimitado por los timestamps
de las lecturas láser, y por tanto a los de los datos de CSM.
Como se observa en la figura 4.6, cada intervalo contiene los seis parámetros finales: la duración o
anchura del intervalo (t_i), las velocidades angulares de las dos ruedas (ωR y ωL) y los tres parámetros
devueltos por el scan-matching, dos para la traslación (sm_x y sm_y) y uno para la rotación (sm_theta).
Estos parámetros, a los que se les llamará parámetros finales, son obtenidos mediante el proceso detallado
en el diagrama de actividad de la figura 4.7.
A continuación se explicará la metodología seguida para el cálculo de los parámetros finales:
a) Calcular el threshold del error medio y de los rayos válidos: Se calcula un límite del error medio
de los rayos (mean_error) y del número de rayos válidos (nvalid) para más tarde poder descartar las
peores lecturas.
b) Leer el siguiente intervalo de CSM: Se leen de forma iterativa los atributos de scan-matching
pertenecientes al siguiente intervalo del actual.
c) Acceder a los timestamps de las lecturas láser referencia (ref) y la siguiente (sens): Se leen los
instantes de tiempo en los que fueron recogidas las lecturas de láser que delimitan el intervalo actual.
d) Obtener los datos de odometría en ref y sens (previamente interpolados): Ya que los datos de la
odometría ya se encuentran interpolados, sólo hay que buscar los datos cuyos timestamps
correspondan con los de ref y sens.
e) Calcular las velocidades de las ruedas en el intervalo actual mediante los datos de odometría:
En este caso se calcularán las velocidades de las ruedas en el intervalo actual a partir de los datos de
la odometría, tal y como se explicó en el apartado 4.2.2A.
f) Calcular las velocidades de las ruedas en el intervalo actual mediante los datos de los encoders:
Igual que en el caso anterior, sólo que ahora se utilizarán los datos procedentes de los encoders (este
proceso también fue explicado en el mismo apartado que en el caso anterior).
g) Filtrado de datos indeseados: Se descartarán aquellos intervalos en los que se detecte que el
vehículos está parado, los datos del scan-matching sean inválidos, el error medio de los rayos sea
mayor que el mean_error calculado en el caso 1, el número de rayos válidos sea menor que nvalid,
también calculado en el caso 1, o que la duración del intervalo sea mayor que un valor máximo
permitido. Este proceso de filtrado de datos será explicado en el apartado 4.2.5.
h) Almacenar datos finales en memoria principal: Por último, para cada uno de los intervalos, se
almacenarán los seis parámetros buscados en memoria principal.
Cuando este proceso acaba, todos los datos por fin estarán listos para ser utilizados por el algoritmo
de calibración. Aún así, tal y como se explica a continuación, se han creado unos métodos para conseguir que
los datos utilizados por el algoritmo de calibración sean lo más óptimos posibles.
4.2.5 Detección y eliminación de datos erróneos
Uno de los mayores inconvenientes de este método de calibración es lo susceptible que es al
tratamiento de datos imprecisos o “outliers”. Éstos dañan considerablemente el proceso de calibración,
produciendo cambios drásticos en la evolución de la estimación de los resultados cuando se producen. Ya
que este método de calibración trabaja con una gran cantidad de datos, puede permitirse la eliminación de un
pequeño porcentaje de datos para así descartar posibles “outliers”. Es por ello que se debe realizar un
esfuerzo en la creación de métodos que permitan limpiar y filtrar este tipo de datos. Las técnicas a las que
nos referimos son presentadas a continuación:
a. Descartar lecturas de láser contiguas parecidas: Existen muchas ocasiones en las que dos lecturas
consecutivas del sensor láser son prácticamente iguales. Esta situación no sólo no favorece a las
técnicas de scan-matching, sino que pueden llegar a empeorar los resultados. Es por ello que antes de
la aplicación de CSM se comprobará si las dos lecturas que se van a utilizar son lo suficientemente
parecidas comprobando que la diferencia entre sus rayos no sea mayor que un umbral épsilon. En tal
caso la nueva lectura será descartada.
b. Filtros en el cálculo de los parámetros finales: En el momento de calcular los parámetros finales,
tal y como se revisó en el apartado anterior, se realizará un importante filtrado de datos para
descartar posibles “outliers”:
1. Threshold del error medio: Para cada uno de los intervalos pertenecientes a CSM se calculará
el error medio por rayo; una vez ordenados los intervalos por el criterio anterior, se podrá aplicar
un porcentaje límite (supongamos 95%) para obtener los intervalos con un mejor error medio por
rayo, pudiendo descartar los restantes (5%).
2. Threshold de los rayos válidos: Procediendo exactamente igual que en el caso anterior, esta vez
los intervalos se ordenarán por el número de rayos válidos que poseen, descartando en este caso
un pequeño porcentaje de los que tengan un menor valor de tal criterio. Teóricamente, en un
scan-matching en el que se utilizan pocos rayos se obtienen peores resultados que con otro con
un número mayor.
3. Vehículo sin movimiento: Si el robot móvil no se está desplazando en un instante, es decir, las
velocidades de las ruedas son igual a cero, este intervalo será descartado al no ofrecer
información útil de la odometría.
4. Intervalo con error cero del scan-matching: Cada intervalo de CSM posee un error producido
en el proceso de scan-matching. Si éste es igual a cero, el intervalo es interpretado como inválido
y será descartado.
5. Duración del intervalo demasiado grande: Si el tiempo empleado en un intervalo es mayor
que un límite (por ejemplo 3 segundos), el intervalo será descartado, pues cuanto más grande es
menor será la precisión en los cálculos de las velocidades de las ruedas.
c. Filtro de “outliers”: Los errores odométricos no-sistemáticos, como los producidos por el
deslizamiento de las ruedas, perjudican de forma importante al proceso de calibración, ya que las
estimaciones de rotación del robot serán distintas en las ofrecidas por el algoritmo de scan-matching
y la odometría. Así, una vez que ha terminado el proceso de calibración, es posible calcular los
errores que existen en los intervalos finales entre la traslación y rotación estimada por los datos de
odometría y los de CSM. Lo ideal sería que los errores fueran lo más cercanos posibles a cero,
indicador de que los datos son fiables; pero en muchas ocasiones las estimaciones de ambas difieren
y es necesario eliminar tales intervalos. Para la realización de estos cálculos, primero se estiman los
parámetros de calibración utilizando todas las medidas disponibles < sk , ωRk , ωR
k >. Ahora, los
errores pueden ser calculados como
ek≙l ⊕ sk− ok⊕l
El Filtro de “outliers” (ver figura 4.6) es por tanto el encargado de limpiar los datos que no sean
correctos o que puedan empeorar los resultados en la estimación de los parámetros. Éste será
ejecutado en cada una de las iteraciones realizada por el algoritmo de calibración iterativo y contiene
dos parámetros que, modificando sus valores, pueden obtenerse distintos resultados. El primero de
estos parámetros es el porcentaje de elementos que se eliminarán de los parámetros finales en cada
iteración. Este valor será muy bajo, ya que es preferible realizar muchas iteraciones y eliminar pocos
elementos que pocas iteraciones y eliminar muchos. Esta afirmación se debe a que si se eliminan
muchos datos en una iteración, también podrían eliminarse datos buenos, empeorando así los
resultados finales, sobre todo al tener pocos datos. En la implementación se utiliza un valor 0.0005;
así, si una muestra tiene 15000 datos, únicamente se eliminarán 7 (15000* 0.0005 = 7.5 ~ 7). Si
además se realizan un total de 30 iteraciones, se eliminarían un total de 210 elementos, asegurando
que prácticamente todos serán “outliers” y que se eliminarán una parte mínima de datos buenos. El
segundo parámetro es el threshold con el que se considera si un intervalo es bueno o no. De modo
que, cuanto menor sea este valor, más elementos serán considerados como inválidos y por tanto más
serán eliminados.
4.2.6 Modos de navegación del robot
En el proceso de recogida de datos es necesario que el robot se desplace por todo el entorno, al ser
posible siguiendo trayectorias variadas para obtener datos muy diversos, tanto de la odometría como del
láser.
A continuación se muestran los distintos modos de navegación utilizados para realizar esta tarea:
a) Telecontrol: Es la forma más básica de mover al robot, ya que es un humano el encargado de
controlar la velocidad y dirección de éste. Esta tarea puede realizarse mediante teclado o joystick,
permitiendo comprobar distintos resultados en la calibración, modificando la trayectoria y
velocidades del robot de forma sencilla. Mediante el modo telecontrol fue como se realizaron las
primeras pruebas, aunque pronto se tuvieron que implementar otros métodos debido a las
desventajas que éste ofrece: la pérdida de tiempo al tener que estar manejando el robot durante toda
la ejecución y la incapacidad de dejar el sistema funcionando durante largos períodos de tiempo de
forma autónoma.
b) Rutas predefinidas: En este modo de navegación, el robot seguirá de forma consecutiva una ruta
previamente definida. Al contrario que en el modo telecontrol, se podrá dejar funcionando el
programa de forma autónoma durante largos períodos de tiempo sin tener que estar pendientes del
robot. Pero por otra parte, resulta peligroso dejar funcionando este método sin supervisión humana
ya que la acumulación de los errores odométricos podrían modificar la ruta predefinida y hacer
colisionar al robot. Se han implementado dos tipos de rutas distintas para poder comprobar cómo se
comporta el algoritmo de calibración en cada una de ellas: la primera, llamada squareRoute, consiste
en recorrer un circuito en forma de cuadrado de 4 metros de largo en ambos sentidos de las agujas
del reloj, tal y como se realiza en el método de calibración UMBmark, citado en el apartado 4.1.1A.
Por tanto, para cada uno de los sentidos, éste método hará avanzar al robot en forma rectilínea en
cuatro ocasiones, y realizará cuatro rotaciones sin desplazamiento. El segundo método, denominado
eightShapeRoute, realiza una trayectoria con una forma parecida a la del número 8, donde en este
caso el robot ejecutará los giros en movimiento e incluso se desplazará con un movimiento lineal
negativo, es decir, marcha atrás.
c) Evitación de obstáculos: Éste es el más complejo de todos pero también el más seguro y utilizado.
Está basado en el algoritmo de evitación de obstáculos Vector Field Histogram (VFH) y fue incluido
de librerías externas de Pal-Robotics. Este algoritmo dirige al robot en línea recta a baja velocidad y,
cuando se encuentra un obstáculo cercano, se aplica un cambio en las velocidades de tal forma que
se evite tal obstáculo y el robot pueda continuar con su trayectoria rectilínea. Este algoritmo, al
contrario que los anteriores, permitió la realización de test largos de forma totalmente autónoma, sin
la posibilidad de que éste pudiera colisionar debido a los errores de odometría, tal y como ocurría
con los modos de rutas predefinidas.
d) Sin traslación: A priori este método carece de sentido, ya que el robot necesita desplazarse para
recoger datos actualizados de la odometría y del láser. Éste será utilizado cuando exista una fuente
externa que sea la que dirija el movimiento del robot. Esta situación puede darse cuando el robot se
encuentra realizando cualquier tarea de navegación y se desea lanzar el algoritmo de calibración al
mismo tiempo. Así, ambos procesos, pueden compartir los mismos recursos y ejecutarse de forma
paralela sin interferir el uno con el otro.
4.2.7 Modos de comportamiento
El programa podrá ejecutarse en dos modos de comportamiento distintos: Log y Online. Su forma de
funcionamiento es distinta pero comparten algunas características, como la utilización del mismo algoritmo
de calibración. También existen otros dos modos de comportamiento secundarios que, si bien no pueden ser
considerados como unos modos nuevos, fueron muy útiles a la hora de realizar pruebas y buscar fallos en el
código. A ellos nos referiremos a continuación.
A. Modo de comportamiento Log
Este primer modo de comportamiento fue el primero que se implementó. En éste, el robot escribirá
de forma continua los datos de la odometría y las lecturas del láser en logs distintos. Esta recogida y
posterior almacenamiento de datos se llevará a cabo durante una duración predeterminada por el usuario o
hasta la interrupción manual del programa. Una vez finalizado este proceso, se podrá lanzar el algoritmo de
calibración en una máquina independiente a la del robot y, utilizando los logs creados, se podrán estimar los
parámetros buscados.
Este modo de comportamiento resultó de gran utilidad en la fase de pruebas, ya que permitía la
ejecución del algoritmo de calibración con diferentes configuraciones y así poder analizar posibles causas y
orígenes de errores. Estos logs también resultaron de gran utilidad a la hora de debugar el código (búsqueda
de errores en la implementación) ya que permitían analizar el instante de tiempo preciso en el que se había
producido un error.
B. Modo de comportamiento Online
El objetivo de este modo de comportamiento es el mismo que el del modo anterior, pero en éste se
busca una calibración mucho más rápida y de forma más eficiente. La idea principal es la de ejecutar el
algoritmo de calibración, esta vez en el propio robot, a medida que se van recogiendo datos de forma
continuada. En este caso no se almacenarán los datos en ficheros de log, sino que serán directamente
guardados en la memoria del programa, permitiendo así acceder a tales datos de forma más rápida y limpia
(la utilización de logs no es una forma eficiente de almacenar datos en aplicaciones de este tipo). Por otra
parte, ahora que se disponen de los datos recibidos por el robot en tiempo real, se aprovechará la potencia del
lenguaje de programación utilizado (C++) para lanzar cada cierto tiempo un hilo de ejecución paralelo que
calculará los parámetros de calibración. Si se estima que la calibración ha llegado a unos niveles aceptables,
este proceso terminará y no será necesario continuar con la ejecución, al contrario que en el modo Log.
Una de las ventajas de éste método es, además de la rapidez con la que se puede calibrar un robot
móvil, la capacidad de ejecución “background”, es decir, el robot puede estar realizando una serie de tareas
específicas y comprobar de forma rápida si los parámetros no están correctamente calibrados y, si así fuera,
restablecerlos con los valores correctos.
C. Modos de comportamiento alternativos: Both y Offline
Estos modos de comportamiento alternativos no ofrecen ninguna característica extra que no
contengan los anteriores, pero que fueron muy utilizados en las fases de pruebas.
El primero de los modos, el Both, reúne en uno las características de los comportamientos Log y
Online. Así, se obtiene la potencia del modo Online con la característica de almacenamiento de logs del
modo Log.
Al igual que en el anterior, se creó otro modo llamado Offline, que tampoco se considerará un modo
nuevo de comportamiento. Con su utilización se leen una serie de logs, previamente guardados por el robot,
con la ventaja de poder recrear una misma situación en cualquier momento sin la necesidad de conectarse
con robots reales o con el simulador. También fue muy utilizado para realizar pruebas y sacar conclusiones,
ya que permitía reproducir todos los tests guardados de forma mucho más rápida.
4.2.8 Modos de calibración
El algoritmo de calibración posee dos modos que permiten tratar los datos de forma diferente y así
obtener resultados ligeramente distintos (ver figura 4.7). Nos referimos al modo incremento y al de partición:
a) Modo de calibración incremento: En este modo de calibración se utilizan todos los datos recogidos
hasta el momento de ejecutar el algoritmo de calibración, independientemente del modo de
comportamiento del sistema.
b) Modo de calibración partición: En este otro modo, los datos recogidos serán divididos en nuevos
grupos de datos, ejecutando el algoritmo de calibración para cada grupo de forma totalmente
independiente al resto. La estimación final será el promedio de los parámetros de todos los grupos de
datos. También será utilizada la varianza de los datos de cada uno de los grupos para conocer si los
datos utilizados convergen hacia un mismo valor.
La ventaja del modo de calibración incremento reside en que para la estimación de los parámetros se
utilizan una gran cantidad de datos, tantos como los que se hayan recogido hasta el momento, mientras que
en el modo partición cada grupo de datos tendrá un número inferior que el total, dependiendo de la cantidad
de grupos que se deseen crear y del número de elementos en cada grupo. Por otra parte, el primer modo de
calibración es más propenso a la acumulación de errores, ya que una serie de medidas malas pueden afectar a
los resultados y arrastrar ese error durante mucho tiempo. Sin embargo, en el modo partición si una serie de
medidas no son correctas sólo afectarán al cálculo de los parámetros que utilice tal grupo, sin perjudicar así a
las calibraciones del resto.
4.2.9 Criterios de parada
La cantidad de datos con las que trabajará el algoritmo de calibración influirá directamente en la
calidad de los resultados. Así, teóricamente, cuantas más muestras se tengan mejores resultados se obtendrán
en el proceso de calibración. Dependiendo del modo de comportamiento del sistema, se pueden diferenciar
dos situaciones distintas:
a) Criterio de parada en el modo de comportamiento Log: En este modo el sistema dejará de
recoger datos cuando se sobrepase un tiempo definido previamente en el código del programa. De
esta forma, si se establece un máximo de 15 minutos, al pasar ese tiempo el robot se detendrá y el
proceso de recogida de datos finalizará, independientemente de la cantidad y calidad de éstos. Esta
solución resulta poco eficiente, ya que si el tiempo establecido es muy grande, se recogerán muchas
cantidades de datos que probablemente no sean necesarios para obtener una calibración aceptable.
En el otro extremo, si el tiempo de recogida de datos es muy pequeño, no se recogerán suficientes
muestras y por lo tanto los resultados no serán buenos.
b) Criterio de parada en el modo de comportamiento Online: En este modo, al contrario que en el
anterior, el sistema dejará de recoger datos, y por tanto finalizará el proceso de calibración, cuando
estime que los resultados obtenidos son lo suficientemente buenos. Para ello se almacenarán todas
las estimaciones obtenidas en cada ejecución del algoritmo de calibración (las pruebas se realizaron
con intervalos de entre 5-10 minutos) y, con ese historial, se podrán tratar para conocer si los
resultados de la calibración obtenida son suficientes. Por otra parte, tal y como se indica en [q], la
varianza describe cómo de lejos se encuentran los valores de una muestra con respecto a la media
aritmética de éstas. Así, en general, la varianza de una población finita de tamaño N puede definirse
como
2= 1N ∑ x i− 2
donde
= 1N ∑ x i
Por tanto, tomando los últimos N resultados devueltos por el algoritmo de calibración, es posible
calcular si la varianza de esas medidas es menor que un límite establecido y, por tanto, observar si
los parámetros no varían mucho para así suponer que están estabilizados. Si las últimas N muestras
de los seis parámetros que se desean estimar tienen una varianza muy baja, se supondrá que tales
parámetros han convergido y por tanto son resultados buenos.
4.2.10Ejecución del método
Una vez explicadas por separado todas las propiedades y configuraciones posibles del método de
calibración, ya es posible reunir todo lo anterior y construir la estructura que tendrá la aplicación final. A
continuación se explicará cómo se aplica el filtro de “outliers” junto a las ejecuciones iterativas del algoritmo
de calibración, y los comportamientos del modo Log y Online.
A. Iteraciones en el algoritmo de calibración
Hasta ahora se ha supuesto que el algoritmo de calibración utilizaba los parámetros finales para la
estimación de los parámetros buscados y, una vez finalizado el proceso, se aceptaban tales resultados como
válidos. Gracias al proceso explicado en el apartado 4.2.5c, es posible la detección y filtrado de “outliers” en
los parámetros finales y ejecutar de nuevo el algoritmo, esta vez con unos datos previsiblemente mejores.
Este método (a partir de ahora algoritmo de calibración iterativo) se podrá ejecutar iterativamente hasta
que en los datos utilizados por el algoritmo no se detecten “outliers” que perjudican tanto al proceso de
calibración. Una vez realizado este proceso, los resultados finales habrán sido obtenidos utilizando unos
datos a priori óptimos y por tanto éstos también deberían serlo.
B. Proceso de la ejecución
El proceso de ejecución, como en casos anteriores, dependerá del modo de comportamiento del
sistema. A continuación se explicarán tales procesos junto a sus diagramas de actividad que ayudarán a su
comprensión:
a) Ejecución en modo de comportamiento Log: En este modo (ver figura 4.10) el robot se centra,
primeramente, y de forma exclusiva, en la recogida de datos del láser y de la odometría. Una vez
finalizado este proceso se aplicará el algoritmo de scan-matching e interpolación de datos a los logs
de láser y odometría, obteniendo así los parámetros finales. Estos serán utilizados a su vez por el
algoritmo de calibración iterativo que, como se explicó anteriormente, se ejecutará tantas veces
como sea necesario hasta que los parámetros finales puedan considerarse libres de “outliers”. Por
último, se obtendrán las estimaciones de los parámetros buscados y por tanto la ejecución finalizará
en este punto.
b) Ejecución en modo de comportamiento Online: Este modo de ejecución, tal y como se observa en
la figura 4.11, es el más complejo pero a la vez el que ofrece mejores resultados en cuanto a
eficiencia y velocidad de cálculo, ya que se ejecutará de forma indefinida hasta que el sistema estime
que ha obtenido unos resultados lo suficientemente óptimos. En este modo el robot recogerá datos de
la odometría y del láser de forma continuada y, cuando el número de datos obtenidos sean
suficientes, se ejecutará el algoritmo de calibración de forma automática. Hay que matizar el hecho
de que la tarea de calibración se realizará en un nuevo hilo de procesamiento, por lo que el robot
puede continuar recogiendo datos mientras el algoritmo de calibración se está ejecutando de forma
paralela. En este modo de ejecución, el método CSM se ejecutará cada vez que se reciba una nueva
lectura láser y el proceso de interpolación de datos justo antes de iniciar el proceso de calibración.
En el caso de que los datos recogidos hasta el momento sean suficientes para iniciar un nuevo
proceso de calibración, se aplicará el filtro de “outliers” junto al algoritmo de calibración iterativo y,
una vez obtenidos unos datos limpios, podrá obtenerse una estimación final de los parámetros
buscados. En este punto se utilizará el criterio de parada explicado en el apartado 4.2.9 y, si los
resultados no llegan a converger y se piensa que aún pueden mejorar, se seguirán recogiendo más
datos, realizando este mismo proceso tantas veces como sea necesario. Si por el contrario, con los
cálculos realizados por medio de la varianza se estima que los resultados son los suficientemente
buenos, la aplicación finalizará y los parámetros serán considerados como los más óptimos posibles,
al menos para la cantidad de datos con la que se está trabajando y para la configuración actual del
sistema.
El modo de ejecución Online resulta mucho más eficiente que el modo Log, ya que podrá finalizar la
calibración cuando estime que los resultados obtenidos son correctos. Además, este método podrá ejecutarse
en el robot junto a otros procesos, pudiendo así compartir recursos y con la posibilidad de calibrar el robot
cuando sea necesario (debido a la detección de descalibración de los parámetros físicos del robot o del sensor
láser, o por cambios en la composición del terreno), no sólo cuando sea indicado explícitamente por el
usuario. Por otra parte, el modo Online es un proceso limpio, ya que no deja restos de su ejecución en la
memoria del robot, al contrario de lo que ocurre en el modo Log.
4.3 Implementación del programa
La última fase de la metodología utilizada es la de implementación, donde se desarrollará un
programa siguiendo las pautas descritas en la fase de diseño. En este apartado se explicarán los aspectos
referentes a esta última etapa: la estructura de la implementación, la descripción de sus principales elementos
y las particularidades o problemas que surgieron en su desarrollo.
4.3.1 Diagrama de clases
Antes de iniciar la implementación del programa se realizó un esquema con todos los módulos que
iban a formar la estructura final. Estos módulos son clases de C++ y juntos crea una jerarquía, tal y como se
observa en el diagrama de clases de la figura 4.12. La creación de esta jerarquía de una forma correcta
permitirá obtener un código final ordenado y bien estructurado, un requisito fundamental cuando se
desarrolla un software de grandes dimensiones y que responda con la máxima eficiencia posible, como es
nuestro caso.
Los diagramas de clases son muy utilizados en Ingeniería del Software, ya que facilitan al
programador la estructura del programa y, por tanto, el código será escrito más rápido y de una forma más
eficiente. En estos se muestran las clases que conforman la implementación, la relación existente entre ellas
y sus atributos y funciones principales. A su vez, este diagrama servirá en esta memoria para obtener una
primera visión de la estructura general del programa.
A continuación (ver figura 4.12) se muestra el diagrama de clases del programa desarrollado:
Se han omitido las clases externas que incluye nuestro programa, ilustrando únicamente las
desarrolladas específicamente para este proyecto y evitando así entrar en detalles de implementaciones que
no son propias. Por otra parte, muchas de las características y comportamientos de nuestras clases ya se
encuentran explicadas en el apartado 4.2 de la fase de diseño, por lo que en numerosas ocasiones se citará el
apartado donde fueron explicadas y se tratará únicamente los aspectos que tengan relación con la
implementación. Además, una explicación exhaustiva de todos los métodos y atributos de las clases carece
de sentido en este tipo de trabajo, por lo que únicamente se citarán y explicarán aquellas que se consideren
que tienen una mayor relevancia para la comprensión del código. También se tratarán aspecto que en el
diseño no se tuvieron en cuenta, como los modos de comportamiento Both y Offline, que, en este capítulo, sí
tiene sentido nombrarlos ya que están incluidos en la implementación
A continuación, se abordará brevemente el objetivo principal de cada una de éstas clases para más
adelante entrar en detalle en sus correspondientes apartados:
a) Clase reemCalibrator: Se trata de la clase principal del programa y la que ejercerá como
organizadora de las tareas. Una de sus principales funciones es la de establecer las opciones de
configuración, que marcarán el comportamiento del programa durante toda la ejecución: modelo de
robot a utilizar, modo de comportamiento, algoritmo de navegación, etc. Esta clase también será la
encargada de controlar los movimientos del robot y de recoger, de forma continuada, los datos
procedentes de la odometría y del sensor láser. La forma en la que se almacenan estos datos o el
modo de estimar los parámetros de calibración, entre otros, dependerá de la configuración
especificada en la llamada al programa, tal y como se explica en el apartado 4.2.3.
b) Clase cal_data: Es la encargada de trabajar con los datos recibidos por la odometría y el sensor
láser. Se definen métodos tales como los necesarios para la escritura y lectura en ambos modos de
comportamiento (Log y Online) o para su procesado mediante CSM e interpolación de datos.
c) Clase cal_method: En esta clase es donde se encuentra implementado el algoritmo de calibración y
todos los métodos utilizados por éste. También se implementan funciones de algunos procesos
descritos en el apartado de diseño, como son el cálculos de los parámetros finales, el filtrado de
“outliers” o el algoritmo de calibración iterativo.
d) Clase tools: Contiene métodos secundarios que serán utilizados por las clases anteriores, evitando
así la duplicación de código y otorgándole un orden lógico a nuestro código.
4.3.2 Ejecución del programa
La ejecución del programa se podrá hacer por línea de comandos, siendo posible determinar algunas
opciones de la aplicación justo en el momento de su iniciación mediante una serie de parámetros. Esto
resulta de gran comodidad para el usuario, ya que no es necesario realizar cambios en el código y volver a
compilar cada vez que se desea modificar la configuración del sistema.
El comando que se debe ejecutar para lanzar el programa, junto a los parámetros de configuración, es
el siguiente (ver tabla 4.4):
Tabla 4.4: Comando para iniciar la ejecución la aplicación
ReemCalibrator --<tipo_robot> --<modo_navegación> --<modo_comportamiento> --<modo_calibración>
A continuación se indicarán cada una de las opciones de los parámetros de configuración:
a) Tipo_robot: Este parámetro permite seleccionar el modelo de la plataforma que se va a utilizar.
1. S1: Modelo de REEM-H1 para el simulador Gazebo.
2. S2: Modelo de REEM-H2 para el simulador Gazebo.
3. H1: Robot real REEM-H1.
4. H2: Robot real REEM-H2.
b) Modo_navegación: Permite seleccionar el modo de navegación del robot.
1. Telecontrol: Los movimientos del robot son controlados por el usuario.
2. Square: El robot sigue una ruta predefinida en forma de cuadrado.
3. Eight: El robot sigue una ruta predefinida en forma de ocho.
4. OA: El robot navega utilizando un algoritmo de evitación de obstáculos.
5. NoTraslation: No se le enviarán comando de movimiento al robot, esperando que esta tarea sea
realizada por un programa externo.
c) Modo_comportamiento: Se elige el modo de comportamiento del sistema, incluyendo en este caso
los alternativos.
1. Log: El robot escribe logs con los datos recibidos y, cuando finaliza esta tarea, ejecuta el
algoritmo de calibración.
2. Online: El robot recoge datos y ejecuta el algoritmo de calibración de forma simultánea.
3. Both: Combinación de los comportamientos Log y Online.
4. Offline: Se leen los datos de ficheros log y se ejecuta el algoritmo de calibración sin conectarse
a ningún robot.
d) Modo_calibración: Permite seleccionar el modo en el que serán tratados los datos en el proceso de
calibración.
1. Incremento: Se utilizan todos los datos disponibles.
2. Partición: Se dividen los datos en distintos grupos, realizando la calibración para cada uno de
forma independiente.
4.3.3 Utilización de varios hilos de ejecución
La implementación creada en este trabajo es multiproceso, es decir, permitirá la creación de más de
un proceso y su ejecución de forma paralela. Se decidió incluir esta funcionalidad para crear un nuevo hilo
de procesamiento cuando en el modo de comportamiento Online se tuviera que ejecutar el algoritmo de
calibración, permitiéndole al robot de este modo seguir con la recogida de datos. Así, ambos procesos
compartirán los recursos del sistema pero trabajarán de forma totalmente independiente.
También hay que tener en cuenta que el proceso principal y el nuevo hilo de ejecución creado no
pueden trabajar sobre una misma instancia de datos, ya que podrían ocurrir errores al intentar leer y escribir
en ellos de forma simultánea. Además, el número de datos utilizados en la calibración deben estáticos, es
decir, debe ser el mismo tanto al inicio como al final de esta tarea. Es por ello que en el momento de la
creación del nuevo proceso todos los datos recogidos hasta el momento serán copiados en un secundario,
pudiendo ser utilizado libremente por el nuevo hilo de ejecución. Aún así, en la copia de los datos principales
se utilizará el recurso “mutex” para evitar los mismos conflictos de memoria comentados anteriormente.
4.3.4 Clase ReemCalibrator
Esta clase, como se explicó en el apartado 4.3.1, será la encargada de organizar las tareas principales
del sistema. Es por ello que incluye en su código las clases cal_data y cal_method, necesarias para el
tratamiento de datos y la calibración del robot. A continuación se realizará una descripción de los parámetros
privados y las funciones públicas implementadas, sin reiterar en las características que hayan sido explicadas
en el apartado de diseño 4.2.
A. Atributos privados de la clase
Los atributos principales de esta clase son:
float _radius, _base: valores nominales de los radios de las ruedas y distancia entre éstas de la
plataforma que se va a utilizar. Éstos no serán utilizados en ningún momento por el algoritmo de calibración,
únicamente para el cálculo de las velocidades de las ruedas por medio de la odometría.
int _encoder_r, _encoder_l: valor actual de los encoders incrementales de los motores.
cal_data::execMode _mode: modo de comportamiento del sistema. Se puede elegir entre el modo
Online, modo Log, modo Both y modo Offline.
cal_method::CalibrationType _calMode: modo de calibración utilizado, pudiendo seleccionar el
modo incremento o el modo partición.
cal_data* _data: instancia de la clase cal_data donde se almacenarán todos los datos requeridos por
el algoritmo de calibración.
cal_data* _data_thread: puntero a una copia del objeto _data que se utilizarán cuando se ejecute un
nuevo hilo de calibración.
robotConnector* _rc: conector con el que nuestro programa podrá comunicarse con los robots.
NavigationSensorDataPtr _laser: instancia utilizada para las comunicaciones con el sensor láser.
Aunque no es propiamente un atributo, en este apartado se explicará una enumeración utilizada porla clase:
Enumeración (enum) movementAlgorithmType: Como se observa en la tabla 4.5, esta
enumeración será utilizada para establecer el modo de navegación que se va a utilizar.
Tabla 4.5: Enum movementAlgorithmTypeenum movementAlgorithmType{
SQUARE = 0,
EIGHT_SHAPE = 1,
OBST_AVOIDER = 2,
UNKNOWN_TYPE = 3
}
B. Métodos públicos de la clase
A continuación se expondrán los métodos de esta clase y sus principales características.
ExecuteCalibration(): Esta es la primera función que se ejecutará en nuestro programa. En primer
lugar se extraerán los parámetros que se incluyen cuando se realiza la llamada a la aplicación, permitiendo
conocer la configuración del sistema que el usuario desea. Las opciones que se pueden variar son las
siguientes:
a) Conectores: Para establecer una comunicación directa con el robot se utilizarán una serie de
conectores, tal y como se vio en el apartado 3.3.4, eligiendo PlayerConnector o
MobileBaseConnector dependiendo si se va a utilizar el simulador Gazebo/Player o un robot real,
respectivamente. En ambos casos se debe especificar el nombre del robot, su dirección IP y el puerto
a través del cual se va a realizar la comunicación.
b) Plataforma: La aplicación ofrece cuatro opciones de conexión: con los dos robots reales REEM-H1
y REEM-H2, y con sus correspondientes modelos en Gazebo, detalladas en el apartado 3.1. Además
se informará sobre la longitud del radio de las ruedas y la distancia entre éstas. Estos parámetros no
se utilizarán en ningún momento en el proceso de calibración, únicamente para estimar las
velocidades de las ruedas mediante odometría.
c) Modo de comportamiento: Se podrá elegir entre los cuatro modos posibles: Log, Online, Both y
Offline (ver apartado 4.2.7). En el caso particular de modo Online y Both se creará un objeto
cal_data secundario. De este modo, cada vez que se lance un nuevo hilo que ejecute el algoritmo de
calibración, todos los datos recogidos hasta el momento serán copiados en este objeto secundario y
el original sólo será utilizado para introducir nuevos datos, tal y como se explicó en el apartado
4.3.3.
d) Modo de navegación: Se puede elegir entre los cuatro modos de navegación: squareRoute,
eightShapeRoute, obstacleAvoider y noTranslation. La explicación de cada uno de estos métodos se
encuentra detallada en el apartado 4.2.6.
e) Modo de calibración: Se puede elegir entre los modos partition e increment, tal y como se explicó
en el apartado 4.2.8.
Una vez configuradas todas las opciones, se iniciará el conector correspondiente para establecer una
conexión con el robot, se inicializará la configuración del láser y se lanzará la función correspondiente al
modo de navegación elegido.
ReemCalibrator(): Se trata del constructor de la clase, donde únicamente se inicializan los atributos
privados.
Las siguientes dos funciones, como se explicó en el apartado 4.2.7, se utilizarán cuando se quiere
realizar una calibración Offline en el primer caso, y en el segundo cuando se utilice el modo de
comportamiento Online:
OfflineExecution(): Esta función implementa el modo de comportamiento Offline. Primeramente se
recogerán las lecturas del láser de un fichero log en formato CARMEN y se les aplicará el algoritmo de scan-
matching CSM, guardando los resultados obtenidos en otro fichero log. Seguidamente se realizará una
interpolación de los datos de la odometría con los timestamps de las lecturas del láser para, por último,
ejecutar el algoritmo de calibración iterativo con los parámetros de configuración pertinentes.
Work(): Es el encargado de lanzar los hilo de ejecución que calcularán los parámetros de
calibración. Previamente se creará un nuevo objeto cal_data, llamado _dataThread, al que se le copiarán los
datos de odometría y scan-matching almacenados hasta el momento en el objeto principal cal_data _data.
Seguidamente se interpolarán los datos de la odometría con los timestamps de las lecturas del láser, se
calcularán los parámetros finales combinando los dos tipos de datos (odometría y láser), y por último se
calcularán los parámetros buscados ejecutando el algoritmo de calibración en modo iterativo.
A continuación se explicarán las funciones que implementan tres de los modos de navegación del
robot. Estos métodos son los que mandan al robot los comandos de las velocidades lineal y de rotación:
SquareRoute(): Esta función realiza un control de la dirección que lleva el robot y el número de
giros realizados (sin traslación lineal), eligiendo un nuevo movimiento en cada iteración dependiendo de
tales variables.
EightShapeRoute(): Muy similar a la anterior, aunque en este caso el número de movimientos
necesarios para finalizar la forma de 8 es mayor, además de que los giros se realizarán en movimiento y se
permiten traslaciones marcha atrás.
ObstacleAvoider(): En este caso se deberán crear primeramente una serie de conectores y servicios
(ver apartado 3.3.4) que permitirán la utilización del algoritmo de evitación de obstáculos VFH. Así, se le
ordenará al robot avanzar en línea recta (con velocidad lineal fija L = 0.2) y, si se detectan obstáculos
cercanos, el algoritmo de evitación de obstáculos le aplicará una influencia (modificación) al vector de
movimiento original, consiguiendo así una nueva trayectoria del robot y la evitación de tal obstáculo.
Las siguientes funciones implementan la forma en que los datos son almacenados, tanto los de
odometría como los del láser (ver apartado 4.2.2):
CheckNewData(): Esta función es llamada dentro de las que dirigen la navegación del robot,
comprobando de forma continua si existen nuevos datos de odometría y de láser. Si es así, y dependiendo del
modo de comportamiento, se realizarán llamadas a los siguientes métodos:
Almacenamiento de los datos de odometría
Si existen datos actualizados de odometría y el modo de comportamiento es Log, tales datos se
escribirán en ficheros logs utilizando la función writeOdometry(). Si por el contrario el modo es Online, los
datos serán introducidos directamente en la memoria del programa llamando a la función
insertOdometryParameters():
a) WriteLogOdometry(): Como se comentó en el apartado 4.2.2A, se han implementado dos formas
de conocer las velocidades de las ruedas: mediante el incremento de los ticks de los encoders, o
mediante los datos de odometría. En el primero, cada vez que existan datos actualizados, se
almacenará el número total de ticks de los encoders, mientras que si los datos provienen de la
odometría, se guardarán las coordenadas absolutas del robot en el instante en que fueron recogidos.
Si el número de ticks de un encoder supera un determinado límite, éstos se inicializarán de nuevo
con un valor inicial 0. Además, estos datos serán almacenado en formato JSON, siguiendo lo
explicado en el apartado 3.3.1A.
b) InsertOdometryParameters(): Ésta función es semejante a la anterior, con la salvedad de que en
vez de guardar los datos en un log, éstos serán almacenados directamente en la memoria del
programa. En concreto se almacenarán en contenedores de C++ de tipo <vector>.
Almacenamiento de los datos del láser
En este caso, si se reciben nuevos datos del láser y el modo de comportamiento es Log, estos serán
guardados en logs mediante la función writeLogLaser(). Por otra parte, si el modo de comportamiento es
Online, éstos serán almacenado en memoria principal llamando a la función insertLaserParameters():
a) WriteLogLaser(): Cada vez que existan nuevos datos procedentes del láser, estos serán
almacenados en un log en formato CARMEN, siguiendo el formato ya explicado en el apartado
3.3.1B.
b) InsertLaserParameters(): Al contrario que pasaba en instertOdometryParameters(), en este caso no
se almacenarán los datos del láser directamente en vectores, sino que se creará un “string” en
formato CARMEN con tales datos. A continuación esa cadena de caracteres se le pasará como
parámetro a la función láser2sm_Online(), donde se aplicará el algoritmo de scan-matching CSM.
Por último, se almacenarán los parámetros calculados en sus vectores correspondientes.
• StopRobot(): como su nombre indica, envía al robot comandos de parada.
• PrintOptions(): Imprime por pantalla las opciones con las que se puede inicial el programa, algo
realmente útil cuando el usuario desconoce el funcionamiento del programa.
4.3.5 Clase cal_data
En esta clase es donde se guardarán los datos recogidos durante toda la ejecución y donde se
aplicarán una serie de procesos para modificar y obtener nuevos tipos de datos a partir de los originales.
Cal_data únicamente incluye código de la clase tools, ya que utilizará métodos implementados en ésta. A
continuación, del mismo modo que se hizo con clase reemCalibrator, se realizará una descripción de los
parámetros privados y las funciones públicas implementadas.
A. Atributos privados de la clase
Los atributos más importantes de esta clase son:
ExecMode _mode: modo de comportamiento del sistema. Proviene de la clase reemCalibrator y se
le asignará tal valor en el constructor de la clase.
FILE *_file_l, *_file_r, *_file_sm, *_file_result: Ficheros utilizados para almacenar los logs del
láser, odometría, scan-matching y los resultados finales.
En esta clase se utilizarán contenedores de C++ de tipo vector (<vector>) para almacenar datos.
Estos puede clasificarse en:
a) Vectores para los atributos de odometría.
b) Vectores para los atributos del scan-matching.
c) Vectores para los parámetros finales.
Por otra parte, al igual que en la clase reemCalibrator, se explicarán en este apartados dos tipos de
enumeraciones (enum):
La primera es una estructura enum con nombre logType (ver tabla 4.6) que permite establecer qué
tipo de log se va a tratar: log de odometría (LOG_R), log de odometría después de interpolar los datos
(LOG_R_INTERP), log de láser (LOG_L) o log de scan-matching (LOG_SM).
Tabla 4.6:Enumeración logType
enum logType{ LOG_R = 0,
LOG_R_INTERP = 1,
LOG_L = 2,
LOG_SM = 3
};
La segunda es otra estructura enum con nombre execMode (ver tabla 4.7) que estable el modo de
ejecución del programa: modo log (LOG), modo Online (ONLINE) y los modos de comportamiento
alternativos Both (BOTH) y Offline (OFFLINE).
Tabla 4.7:Enumeración execMode
enum execMode{
LOG = 0,
ONLINE = 1,
BOTH = 2,
OFFLINE = 3,
DEFAULT = 4
};
También se han implementado unas estructuras (extraídas del código perteneciente a CSM, por lo
que al no ser un desarrollo propio no se mostrará su código) que servirán para establecer los parámetros de
configuración del algoritmo de scan-matching CSM:
a) La primera es la estructura ld_noise_param, donde se definen los parámetros de configuración de
la aplicación de ruido en los parámetros de odometría asociados a cada una de las lecturas láser.
Destacan sigma_xy, que es utilizado para la aplicación de ruido en las coordenadas de traslación,
sigma_theta para la aplicación de ruido en la rotación, y “seed”, que es una “semilla” a partir de la
cual se generan número aleatorios.
b) La segunda es p (parameters) que contiene los parámetros de configuración utilizados por el
algoritmo de scan-matching. Contiene atributos como el número máximo de iteraciones permitidas
en CSM, el rango máximo y mínimo para los rayos del láser, y el tipo de algoritmo de scan-matching
que se va a utilizar.
B. Métodos públicos de la clase
A continuación se enumerarán y detallarán cada uno de los métodos de esta clase. El primero se trata
del constructor:
Cal_data(execMode mode): Es el constructor de la clase y en éste se inicializan las variables
privadas. También se abren algunos ficheros, como el necesario para escribir los resultados obtenidos por el
algoritmo de calibración, o los utilizados por el modo de comportamiento Log para escribir los datos de
odometría y del láser. Recibe el parámetro “execMode mode”, donde mode es el modo de comportamiento
en el que se está ejecutando el programa y que será utilizado para establecer distintos tipos de
configuraciones.
A continuación se introducen dos funciones para copiar datos de un objeto cal_data a otro:
Void copyParametersVectors(cal_data *data): Esta función, debido a la problemática que surge de
compartir datos entre distintos hilos de ejecución (ya explicado en el apartado 4.3.3), será la encargada de
realizar la copia de los vectores de odometría y CSM. Además, esta función recibirá como parámetro el
objeto cal_data original que se desea copiar.
Void copyFinalParametersVectors(cal_data *data): El funcionamiento de este método es similar a
la anterior, con la salvedad de que en este caso se copiarán únicamente los vectores de los parámetros finales.
Para leer los parámetros de un fichero log se ha creado una sola función con el objetivo de no
implementar una para cada tipo de log y evitar así la duplicación código:
Void read_log(logType log): Exclusivamente utilizada en el modo de comportamiento Log, ésta
función lee un fichero log, extrae los parámetros guardados en éste y los almacena en la memoria del
programa, concretamente en sus vectores correspondientes. Esta tarea es muy importante ya que el algoritmo
de calibración deberá acceder a estos datos en numerosas ocasiones y, como es evidente, la obtención de
información almacenada en memoria resulta mucho más rápido que obtenerla a partir de sucesivas lecturas
de un fichero. En esta función se establecen tres opciones distintas, en función del parámetro de entrada
“logType log”, que indica de qué tipo de log se va a leer:
• LOG_R: Lee todos los parámetros de un log de odometría (segundos y microsegundos del
timestamp, los tres parámetros de odometría x, y y theta, y los valores de los encoders) y los
almacena en sus vectores correspondientes en memoria principal.
• LOG_R_INTERP: Igual que en el anterior, sólo que en este caso los datos de odometría se leen de
un fichero donde ya se ha realizado la interpolación de datos.
• LOG_SM: En este caso se lee el fichero donde están guardados los resultados del algoritmo de
scan-matching. Los parámetros del log de CSM (validez de la lectura, parámetros de la roto-
traslación x, y y theta, número de rayos válidos, segundos y microsegundos de las dos lecturas del
láser y el error) serán de nuevo almacenados en sus vectores correspondientes en memoria principal.
A continuación se presentarán las funciones referentes al algoritmo de scan-matching CSM. El
código pertenece a la librería CSM (ver apartado 3.3.3) y tuvo que incluirse en nuestra implementación ya
que se tuvieron que realizar algunos cambios, sobre todo para adaptar el código al modo de comportamiento
Online. Entre estos cambios destacan la lectura de datos por medio de un “string” en vez de por fichero, la
escritura de resultados en contenedores de tipo <vector> en vez de en logs, y la capacidad de poder
almacenar la lectura de láser que se está utilizando para que la próxima vez que se llame al método pueda ser
utilizada como “la lectura anterior”, evitando así la utilización de logs. Las funciones implementadas son las
siguientes:
Void laser2sm_log(): Llama a la función ld_remove_doubles_log() y, cuando haya terminado todo
el proceso, cierra el fichero donde se han escrito los resultados.
Void laser2sm_online(const string &s): Se inicializarán algunas opciones de la configuración y se
llamará a ld_remove_doubles_online(), obteniendo como parámetro un “string” con la lectura del láser
actual.
Int ld_remove_doubles_log(): Primeramente, si se acaba de entrar en este método, se inicializarán
algunas opciones de la configuración. Después, para cada uno de los elementos del log donde se encuentran
las lecturas del láser, se descartarán las lecturas que no cumplan el formato correcto junto a las que sean muy
parecidas a la lectura anterior, y se llamará a la función ld_slip() para añadirle ruido a los parámetros de
odometría asociados a la lectura actual. Una vez recorridas todas las lecturas se liberará la memoria de los
objetos creados que ya no se van a volver a utilizar, y se imprimirán por pantalla algunas estadísticas del
proceso, como el número total de lecturas, las invalidas y las descartadas. Esta función devuelve el número
de lecturas inválidas, es decir, aquellas con un formato incorrecto.
Int ld_remove_doubles_online(const string &ld_str): Similar al método anterior, sólo que en este
caso únicamente se trata una lectura en formato de “string”, pasada por parámetro, y sin la utilización de
ficheros log. Esta función devuelve el valor 0 si la lectura láser tiene un formato incorrecto, 1 si es la primera
vez que se ejecuta el método o si finaliza de forma satisfactoria, y para el resto de casos se retornará el
número de lecturas inválidas que, al igual que en el método anterior, son aquellas con un formato incorrecto.
Int ld_slip(const LDP &ld_data): Atendiendo a los parámetros de configuración y utilizando una
desviación típica de ruido gaussiano, añade una determinada cantidad de error a los datos de odometría
asociados a las lecturas y, por tanto, alterando el resultado final. Esta función recibe como parámetro ld_data
de tipo LDP, que contiene toda la información de una lectura láser, y retorna el valor -1 cuando el formato de
la lectura láser es inválido y 0 si el proceso se realiza correctamente.
Int ld_sm2(const LDP &ld_data): Tras la inicialización de los parámetros de configuración se
procederá al cálculo de los parámetros pertenecientes a la roto-traslación del robot. Primeramente se utilizará
una lectura del láser y la anterior a ésta para ejecutar el algoritmo de scan-matching (normalmente PL-ICP).
Una vez realizados todos los cálculos, se almacenarán los datos dependiendo del modo de comportamiento
del programa, en un fichero log si el algoritmo se está ejecutando en modo Log, o en la memoria principal si
se está trabajando en modo Online. Este método, al igual que el anterior, recibe el parámetro ld_data de tipo
LDP y retorna un valor de -1 si no fue capaz de leer la primera lectura o si el algoritmo a utilizar es
desconocido, -2 si la primera lectura es inválida, el número de elementos del fichero de scan-matching que
tienen un formato inválido, 2 si ha ocurrido un fallo en un matching realizado por ICP y, por último, 0 si el
proceso ha finalizado de forma correcta.
int ld_equal_readings(const LDP &ld1, const LDP &ld2, double epsilon): Esta función es la
encargada de comprobar cómo de parecidas son dos lecturas de láser consecutivas. Ya que cada lectura de
láser se compone de una serie de rayos, éstas se comprarán restando los valores de todos sus rayos para así
comprobar si alguno de los valores obtenidos es mayor que épsilon, asumiendo por tanto que las lecturas son
diferentes. Si ningún valor obtenido en las restas es mayor que épsilon, esto significa que ambas lecturas son
muy parecidas y por tanto hay que descartar una de ellas.
Ya que es un método muy sencillo, aunque complicado de resumir, en la tabla 4.8 se muestra el
pseudo-código de esta función para obtener un mejor entendimiento.
Tabla 4.8:Pseudo-código de la función ld_equal_readings() LDP r1, r2 //instancias de las lecturas
N = 181 //numero de rayos de una lectura láser
épsilon = 0.03 //error mínimo entre dos rayos
i = 0 //rayo actual
Mientras i < N //hasta que no se hayan comprobado todos los rayos
{
si (r1(i) es nulo o r2(i) es nulo ) //si el rayo i de r1 o r2 es nulo se
siguiente lectura; //continua con el siguiente rayo
si ( abs( r1(i) – r2(i) ) > épsilon) //si el valor absoluto de la resta entre
FIN; //los rayos i de r1 y r2 es mayor
//que épsilon, se puede asumir que los rayos son distintos y
//por tanto que las lecturas r1 y r2 son distintas
i = i + 1 //se incrementa el número de rayo actual
}
Este método recibe como parámetros las dos lecturas láser que se quieren comparar, en formato LDP
junto al error épsilon. Por otra parte, devolverá 0 si las lecturas se consideran distintas o 1 en caso contrario.
Void sm_options(struct sm_params*p, struct option*ops): Esta función únicamente inicializa
algunos parámetros de configuración del proceso de scan-matching.
Void interpolationOdometryLog(): Esta función implementa el proceso de interpolación descrito
en el apartado 4.2.3B. A continuación, en la tabla 4.9 se explicará el pseudo-código seguido en esta función:
Tabla 4.9: Pseudo-código de la función interpolationOdometryLog()
open(log_interpolacion) //se crea un fichero log donde se guardarán los nuevos //datos interpolados
mientras queden lecturas láser por leer{
L = lectura láser actual
R1 = find_closest() //lectura de odometría más cercana y menor que L
R2 = find_closest() //lectura de odometría más cercana y mayor que L
factor = valor entre [0,1] //peso que tiene L en el intervalo [R1,R2]
//Cálculo de los nuevos parámetros odométricos en L
segundos = L.segundos //en el mismo instante que L
microsegundos = L.microsegundos //en el mismo instante que L
ticks_izq = interpolación(factor, R1, R2)
ticks_dcho = interpolación(factor, R1, R2)
odom_x = interpolación(factor, R1, R2)
odom_y = interpolación(factor, R1, R2)
odom_theta = Normalizar ( interpolación(factor, R1, R2) )
write (log_interpolacion) //escritura del nuevo dato en el log de
//interpolación
}
Como se observa en la tabla anterior, hay que tener especial cuidado en el cálculo interpolado del
ángulo theta, ya que podría darse el caso de que un ángulo fuera cercano a PI y otro a -PI, obteniendo
ángulos fuera del intervalo de trabajo [-PI,PI]. La solución a este problema es muy simple, se deberán
normalizar los ángulos obtenidos, tanto en las operaciones intermedias como en el resultado final. El
pseudo-código y una breve explicación a este método se encuentran desarrollados en el apartado 4.3.8.
En las primeras versiones de éste método también se incluían en el fichero log, además de los datos
interpolados, medidas odométricas originales. Esto se hizo para entender bien el funcionamiento del
programa y buscar posibles fallos en la implementación pero, ya que en este punto esto carece de sentido,
únicamente se incluirán los datos interpolados, que son los únicos que utilizará el algoritmo de calibración.
Void interpolationOdometryOnline(): Esta función tiene un comportamiento similar al anterior; la
única diferencia radica en que en este caso los datos no se almacenarán en un fichero log, sino que lo harán
directamente en los vectores apropiados en memoria principal.
Const int find_closest(float ts, int start_at): Este método se utiliza para encontrar las medidas de
odometría más cercanas a una lectura láser. Como se observa en la figura 4.13, a la función se le pasan dos
parámetros: ts, que es el timestamp de la lectura de láser y start_at, que guarda el índice del timestamp del
último dato de odometría interpolado. Start_at fue implementado por motivos de eficiencia, ya que una vez
que se tiene un nuevo dato interpolado, las nuevas interpolaciones siempre se van a hacer con lecturas
posteriores a ésta, por lo que no tiene sentido recorrer las medidas odométricas inferiores. El algoritmo
básicamente recorre todos los datos de odometría posteriores a start_at buscando dos medidas: las que se
encuentran en el instante anterior y posterior a ts. Por último, esta función devuelve el índice de la lectura
odométrica buscada o un valor de -1 en caso de error.
Cuando se está trabajando en una implementación de dimensiones considerables, es necesario tener
cuidado en la forma en cómo se guardan y se accede a los datos. El código que se está implementando es
probable que vaya a ser utilizado por segundas personas y podrían realizar un tratamiento erróneo de los
datos. Encapsulando el código, es decir, ofreciendo métodos específicos para la escritura y lectura de los
datos privados de una clase, damos la posibilidad de modificar y obtener únicamente los datos que resulten
de interés, facilitando la tarea al usuario y evitando posibles errores realizados de forma involuntaria. Las
funciones que contienen en su nombre get devuelven el valor especificado en el nombre de la función,
mientras que las funciones con set modifican el valor con el parámetro que se le pasa a la función. No se
explicará cada uno de estos métodos, pues en el nombre de la función se puede observar la variable que se
pretende tratar.
Estas funciones se pueden implementar dependiendo del tipo de datos que traten:
a) Lectura de los datos odométricos desde logs: Gracias al parser Json-spirit (ver apartado 3.3.3) es
posible extraer cualquier parámetro del log de odometría.
b) Lectura de los datos odométricos desde vectores: Se accederá a los vectores mediante la función
at() para obtener el parámetro de odometría buscado.
c) Escritura de los datos odométricos en vectores: Los datos de odometría se insertarán utilizando la
función push_back() (a modo de pila) en sus respectivos vectores.
d) Lectura de los datos de scan-matching desde logs: Se obtendrán los datos de CSM mediante el
parser Json-spirit.
e) Lectura de los datos de scan-matching desde vectores: Se accederá a vectores mediante la función
at() para obtener los datos de CSM.
f) Escritura de los datos de scan-matching en vectores: Los parámetros calculados por CSM serán
insertados con la función push_back() en sus vectores correspondientes.
g) Escritura de los parámetros finales en vectores: Los parámetros finales, obtenidos al combinar los
datos de odometría y de CSM, serán almacenados en sus vectores correspondientes mediante la
función push_back().
h) Lectura de los parámetros finales desde vectores: Los parámetros finales serán leídos de sus
vectores con la función at().
4.3.6 Clase cal_method
En esta clase se implementan los métodos necesarios para la realización de la calibración, funciones
para la obtención de los parámetros finales y otras utilizadas para filtrar los datos erróneos que puedan
perjudicar al proceso de calibración.
A. Atributos de la clase
Los principales atributos de esta clase son:Double _est_d, _est_r_l, _est_r_r, _est_laser_x, _est_laser_y, _est_laser_theta: estimación de los
parámetros calculados por el algoritmo de calibración.
Cal_data * _data: puntero al objeto cal_data desde donde se recogerán los datos que se utilizará
para la calibración.
Vectores donde se guardarán los resultados cada vez que se ejecute el algoritmo de calibración:
Esto sólo tiene sentido en el modo de comportamiento Online, ya que en Log únicamente se realizará la
calibración una vez.
En esta clase, como en las anteriores, también existen dos tipos de enumeraciones:
OdometryDataType: Informa sobre la fuente de los datos utilizados para calcular las velocidades
angulares de las ruedas (ver tabla 4.10).
Tabla 4.10: Enumeración OdometryDataType
enum OdometryDataType{
ENCODERS = 0,
ODOMETRY = 1
};
CalibrationType: Indica si el algoritmo seguido por el algoritmo de calibración es increment o
partition (ver tabla 4.11).
Tabla 4.11: Enumeración CalibrationType
enum CalibrationType{
INCREMENT = 0,
PARTITION = 1
};
B. Métodos de la clase
A continuación se enumerarán y detallarán cada uno de los métodos de esta clase:
Cal_method(cal_data *data): Se trata del constructor de la clase. Únicamente crea un punteo al
objeto cal_data que se le pasará como parámetro.
Void merge_data ( OdometryDataType odom_type, float radius = 0, float base = 0): Es el
encargado de fusionar los datos de odometría y del scan-matching para el cálculo de los parámetros finales.
Este proceso, explicado en el apartado 4.2.4, recorre con un bucle for todos los intervalos de los datos de
CSM para calcular las velocidades en cada uno de éstos.
Tales velocidades se calcularán de dos modos distintos en función de la fuente de los datos: si
proceden de los ticks de los encoders las velocidades angulares se calcularán llamando a la función
wheelsVelocitiesEncoders(), mientras que si proceden de la odometría se llamará al método
wheelsVelocitiesOdometry(). También se realizará un filtrado de las lecturas que previsiblemente puedan
contener “outliers”, descartando aquellas que los contengan y continuando con la evaluación del siguiente
intervalo de CSM. Los parámetros finales serán almacenados en los vectores finales a los que se podrá
acceder más tarde.
Esta función obtiene por parámetros el modo de calcular las velocidades de las ruedas (mediante las
medidas de los encoders u odometría) y el radio de las ruedas y la distancia entre éstas, utilizadas por las
funciones wheelsVelocitiesOdometry() y wheelsVelocitiesEncoders().
Void wheelsVelocitiesOdometry(float &left_speed, float &right_speed, int jb_ref, int jb_sens,
float t_i, float radius, float base): Este método permite calcular las velocidades angulares de las ruedas del
robot mediante los datos de odometría, tal y como se comentó en el apartado 4.2.2A. Los datos de entrada
son los índices de las dos lecturas láser que delimitan el tamaño del intervalo, la duración de tal intervalo en
segundos y las medidas nominales del robot de los radios de las ruedas y la distancia entre éstas. Como
parámetro de salida se tienen los cálculos de las velocidades de ambas ruedas.
Void wheelsVelocitiesEncoders(float &left_speed, float &right_speed, int jb_ref, int jb_sens,
float t_i, float radius, float base): Este método tiene el mismo objetivo que el anterior, con la diferencia de
que las velocidades se calcularán por medio de los ticks incrementales de los encoders. Los parámetros de
entrada y de salida también son los mismos que en wheelsVelocitiesOdometry().
Int find_closest(float ts, int start_at): El comportamiento de este método es básicamente el mismo
que el de la función con el mismo nombre implementada en la clase cal_data. Se utiliza para encontrar, en
este caso, los datos de la odometría recogida en el mismo instante que una lectura láser. Hay que tener en
cuenta que en este caso los datos de odometría ya se encuentran interpolados, por lo que todas las lecturas de
láser tienen asignada unos datos de odometría.
Void full_calibration(): Este método podría calificarse como el núcleo del programa, ya que es
donde realmente se realiza la estimación de los parámetros que se desean calibrar. En este se desarrollan las
ecuaciones explicadas 4.2.1 y no se explicarán, ya que se trata de una traducción de las ecuaciones
matemáticas al lenguaje C++. Cabe destacar que para las operaciones con matrices se utilizará la librería
Eigen, introducida previamente en el apartado 3.3.3.
VectorXf full_calibration_min(): Implementa la parte matemática descrita en el “Resolución del
problema de mínimos cuadrados con restricciones” del apartado 4.2.1. Devuelve un vector con una solución
o uno con sus elemento a cero si la solución de las raíces del polinomio de segundo grado es imaginaria.
Void calib_iterative(CalibrationType calMode, std::ofstream &result): Esta función implementa
el algoritmo de calibración iterativo explicado en el apartado 4.2.10A. Su comportamiento depende tanto del
modo de comportamiento del programa (modo Online, Log, Both u Offline) como del modo de calibración
(modo incremento o partición). Esta función, dependiendo del resultado devuelto por compute_errors(),
llamará a full_calibration() tantas veces como sea necesario hasta que el error devuelto sea menor que un
threshold (constante y definido por el usuario). Cal_iterative() recibe por parámetros el modo de calibración
y un stream donde se almacenarán los resultados obtenidos en cada llamada a full_calibration().
Int compute_errors(double threshold): Este método implementa el proceso descrito en el apartado
4.2.5c. Calcula si en los parámetros finales se encuentra algún tipo de “outlier”, y en caso afirmativo los
elimina. Obtiene por parámetro el threshold que los errores de la traslación y la rotación tomarán como
referencia para saber cómo de buenas son tales medidas, y devolverá 1 en el caso de que las medidas se
encuentren relativamente limpias o 0 en caso contrario.
Void saveData(): La función de este método consiste en apilar en una serie de vectores la última
estimación realizada por el algoritmo de calibración. Esto resulta útil para obtener un historial de todos los
resultados obtenidos y podrá ser utilizado para dibujar gráficas o para obtener estadísticas del proceso.
Void oplus(double a1, double a2, double a3, double b1, double b2, double b3, double &r1,
double &r2, double &r3): Este método realiza la operación de composición de transformaciones definida
en [1]. Recibe por parámetros los atributos x, y y theta de la roto-traslación procedentes dos fuentes distintas
y devuelve los resultados en el mismo formato que las entradas.
Void plotStatistics(): Muestra por pantalla 6 gráficas (ver figura 4.14), una por cada parámetro que
se quiere estimar, con el progreso de su estimación en tiempo real. De esta manera resulta sencillo observar
los comportamientos que siguen y así poder comprobar, en tiempo real, si la calibración se está realizando
correctamente. La frecuencia con las que las gráficas se actualizan dependen de la frecuencia con la que se
lanzan los hilos de calibración en el modo Online.
4.3.7 Clase tools
Esta clase implementa una serie de métodos genéricos que pueden ser utilizados por cualquiera otra
que la incluya. Los métodos implementados son los siguientes:
Int roots(double a, double b, double c, double* x1, double* x2): Calcula las raíces de un
polinomio de segundo grado. Recibe por parámetros los atributos del polinomio y retorna el valor 1 en caso
de que la solución sea real, o 0 si es imaginaria. Además, los parámetros calculados serán almacenado en los
punteros x1 y x2.
Float tv2sec(float sec, float usec): Transforma un timestamp compuesto por 2 elementos, segundos
y microsegundos, en segundos en formato coma flotante.
VectorXf x_given_lambda(const MatrixXf* M, float lambda,const MatrixXf* W): Utilizado por
la función full_calibration_min(), este método realiza una serie de operaciones necesarias en el algoritmo de
calibración, entre las que destaca el cálculo del eigenvalor de una matriz, incluido en la librería Eigen.
Float normalize_theta(float theta_): Por normalización de ángulos entendemos el proceso de
calcular el valor de un ángulo dentro de un rango en concreto; en nuestro caso (estándar en robótica móvil) el
robot podrá rotar desde el ángulo -π a π. En la tabla 4.12 se propone el pseudo-código de éste sencillo
proceso.
Tabla 4.12: Pseudo-código de normalize_theta()
//sea theta el ángulo que se quiere normalizar
si theta < -M_PI
theta = theta + 2 * PI
si theta > M_PI
theta = theta - 2 * PI
De este modo nos aseguramos que todos los ángulos con los que se trabaja estarán dentro del
intervalo [-π,π].
Int sign(float value): Función muy simple que devuelve el signo de un valor en coma flotante
pasado por parámetro. Implementado únicamente por razones de optimización y claridad en el código.
Capítulo 5: Pruebas y resultados
Capítulo 6: Conclusiones y trabajos futuros
6.1 Conclusiones
6.2 Trabajos futuros
Bibliografía
[a] Censi, A., Marchionni, L., Oriolo, G. (Mayo 2008): Simultaneous maximum-likelihood calibration of
odometry and sensor parameters.
[b] Meyers, S. (xxxx): 55 Specific Ways to Improve Your Programs and Designs.
[c] Ribeiro, M.I., Lima, P. (Abril 2002): Kinematics Models of Mobile Robots. Instituto Superior
Técnico (IST), Instituto de Sistemas e Robótica (ISR), Lisboa, Portugal.
[d] Sanfeliu Cortés, A., Andrade Cetto J. (2009): Modelo Cinemático y Dinámico de Robots Móviles.
Curso Master/Doctorado, Robótica Móvil y Navegación.
[e] Siegwart, R., Nourbakhsh I. R. (2004): Introduction to Autonomous Mobile Robots. A Bradford
Book, The MIT Press, Cambridge, Massachusetts, London, England.
[f] Sanfeliu Cortés, A. (2009): Sensores. Curso Master/Doctorado, Robótica Móvil y Navegación.
[g] Chakarov, D. (xxxx): Kinematic Model of Nonholonomic Wheeled Robots for Mobile Manipulation
Tasks. Institute of Mechanics- BAS, 1113 Sofia, “Acad. G. Bonchev” Str., Block 4.
[h] Ollero Baturone, A. (2001): ROBÓTICA: Manipuladores y robots móviles. ISBN: 84-267-1313-0.
[i] Borenstein, J. And Feng, L. (Diciembre 1994): UMBmark – A Method for Measuring, Comparing,
and Correcting Dead-reckoning Errors in Mobile Robots.
[j] Borenstein, J. And Feng, L. (1996): Gyrodometry: A new Method for Combining Data from Gyros
and Odometry in Mobile Robots. The University of Michigan. Advanced Technologies Lab, 1101
Beal Ave. Ann Arbor, MI 48109-2110.
[k] Dudek, G., y Jenkin, M. (Febrero 2000): Computacional Principles of Mobile Robotics. ISBN-13:
9780521568760
[l] Kelly, A. (2004): Fast and Easy Systematic and Stochastic Odometry Calibration. Robotics Institute,
Carnegie Mellon University, Pittsburg, PA 15213-3890
[m] Martinelli, A., Tomatis, N., Tapus, A. and Siegwart, R. (2003): Simultaneous Localization and
Odometry Calibration for Mobile Robot. Autonomous Systems Lab Swiss Federal Institute of
Technology Lausanne (EPFL). Lausanne, Switzerland
[n] Antonelli, G., Chiaverini, S. and Fusco, G. (Octubre 2005): A calibration method for odometry of
mobile robots based on the least-squares technique: theory and experimental validation. IEEE
Transactions on Robotics, vol. 21, pp. 994–1004,
[ñ] Von der Hardt, H. J., Husson, R. and Wolf D. (Mayo 1998): An automatic calibration method for a
multisensor system: application to a mobile roboert localization system. in Proceedings of the IEEE
International Conference on Robotics and Automation, vol. 4, (Leuven, Belgium), pp. 3141– 3146.
[o] Borenstein, J. y Feng, L. (Diciembre 1996): Measurement and correction of systematic odometry
errors in mobile robots. IEEE Transactions on Robotics and Automation, vol. 12.
[p] Censi, A. (2008): An ICP variant using a point-to-line metric.
[q] Steiner E. (2005): The Chemistry Maths Book. Oxford University Press Inc., New York, U.S.A.
ISBN: 84-291-5159-1
[r] Meyers, S. (xxxx): Effective C++: 55 Specific Ways to Improve Your Programs and Designs (3rd
edition). Addison-Wesley Professional Computing Series.
[s] Meyers, S. (xxxx): Effective STL: 50 Specific Ways to Improve Your use of Standard Template
Library (3rd edition). Addison-Wesley Professional Computing Series.
[t] Borenstein, J. (1995): Control and kinematics design of multi-degree-of-freedom mobile robots with
compliant linkage.
[u] Martinelli, A. y Lamon, P. (2010): Simultaneous Odometry and Range Sensor Calibration.
[v] Doh, N, Choset, H. y Chung, W. K. (2003): Accurate Relative Localization Using Odometry.
[w] Komoriya, K. Y Oyama, E. (2002): Position estimation of a mobile robot using optical fiber
gyroscope (OFG).
[x] Roy, N. y Thrun, S. (1999): Online self-calibration for mobile robots.
[y] Goel, P, Roumeliotis, S. I. y Sukhatme, G. S. (1999): Robot localization using relative and absolute
position estimates.
[z] Antonelli, G. y Chiaverini S. (2007): Linear estimation of the physical odometric parameters for
differential-drive mobile robots.
[aa] Asti Vera, A. (1972): Metodología de la investigación. Barcelona, Herder.
[bb] Bunge, M. (1981): La investigación científica. Barcelona, Ariel
Páginas Web Consultadas[w1] http://playerstage.sourceforge.net/
[w2] http://www.pal-robotics.com/
[w3] http://eigen.tuxfamily.org/
[w4] http://www.cds.caltech.edu/~andrea/research/sw/csm.html
[w5] http://www.codeproject.com/KB/recipes/JSON_Spirit.aspx
[w6] http://www.boost.org/
[w7] http://www.pal-robotics.com/
[w8] http://www.scilab.org/
[w8] http://www.json.org/
[w9] http://www.cds.caltech.edu/~andrea/research/calibration.html
[w10] http://live.gnome.org/Dia
[w11] Real Academia Española [en línea], http://www.rae.es [Consulta: 18/08/2010]
[w12] ¿Cuál es el futuro de los robots inteligentes?, [en línea]
http://www.wharton.universia.net/index.cfm?fa=viewArticle&ID=726 [Consulta: 18/08/2010]
[w13] http://www.ubuntu.com/
[w14] http://www.oselas.com/
[w15] http://valgrind.org/
[w16] http://www.cmake.org/
[w17] http://www.kdevelop.org/
