Desarrollo de Algoritmo Basado en Inteligencia Artificial y Slam
Desarrollo de Algoritmo Basado en Inteligencia Artificial y Slam
Desarrollo de Algoritmo Basado en Inteligencia Artificial y Slam
AREQUIPA - PERÚ
2018
1
AREQUIPA - PERÚ
2018
2
AGRADECIMIENTO
A mis padres por haberme apoyado brindandome su confianza, así como ser mi modelo a seguir y
brindarme sus consejos en todo momento.
A mis hermanos Carlos, Jose y Gustavo por ser las estrellas de mi vida, la fuente de mis alegrías y el
motivo de superación.
A mi enamorada Valeria por haberme brindado su apoyo incondicional desde el primer día, así
como enseñarme a tener convicción para realizar mis metas .
A la Universidad Nacional de San Agustin, mi alma máter por haberme brindado todas las bases y
experiencia para poder desarrollarme como profesional.
Al Ing. Lizardo Pari por brindarme sus apoyo en el desarrollo del proyecto y sus consejos para el
mejor desarrollo de este.
Finalmente, les doy gracias a mis amigos y compañeros quienes fueron pieza fundamental en el
desarrollo del tema
Percy
3
DEDICATORIA
RESUMEN
La característica de los nuevos sistemas de robots es que operan cada vez más desestructurados, es
decir en ambientes que son impredecibles, para ello los robots están dotados de sensores que dan
información del entorno en tiempo real, para poder realizar la navegación autónoma.
Hoy en día la robótica tiene entre sus principales objetivos dotar al robot de autonomía, con la
finalidad de que los robots sean capaces de navegar de forma autónoma en los lugares de geografía
desconocida, es así que el presente trabajo proporciona una amplia visión del problema de
Localización y Modelado Simultaneo o SLAM “Simultaneous Localization and Mapping” que es el
método para poder desarrollar la lógica de darle la referencia del lugar donde se encuentra el Robot
y tener un barrido de la zona alrededor de este mismo.
Para poder desarrollar el algoritmo se requirió de la teoría de probabilidades y las librerías de Ros
este nos permite integrar el sistema operativo Ubuntu con los programa Rviz que es una interfaz
gráfica para visualizar los resultados y Gazebo que es un simulador de robots el cual cuenta dentro
de sus librerías a robots usados hoy en día por instituciones de investigación.
El desarrollo del algoritmo proporcionara mapas de ambientes simulados a partir de los datos
proporcionado por los sensores, con la finalidad de que este trabajo pueda ser aplicado a los robots
autónomos es que se trabajara con una placa Upboard que es un computador de placa reducida para
uso de desarrollo.
Los resultados obtenidos nos muestran que el algoritmo logre su cometido de darle autonomía al
robot generando un mapa mostrando su ubicación este en el entorno graficado y que pueda trabajar
en el sistema embebido (Upboard) para uso de más aplicaciones.
ABSTRACT
The characteristic of the new systems of robots is that they operate increasingly unstructured, that is
to say in environments that are unpredictable, so that the robots are equipped with sensors that give
information of the environment in real time, to be able to carry out autonomous navigation.
Nowadays, robotics has among its main objectives to provide the robot with autonomy, with the aim
that robots are able to navigate autonomously in places of unknown geography, this is how the
present work provides a broad vision of the problem of Location and Simultaneous Modeling or
SLAM "Simultaneous Localization and Mapping" which is the method to be able to develop the
logic of giving the reference of the place where the Robot is located and to have a sweep of the arid
zone of the same.
To be able to develop the algorithm it was required the theory of probabilities and libraries of Ros
this allows us to integrate the Ubuntu operating system with the Rviz program which is a graphical
interface to visualize the results and Gazebo which is a robot simulator which counts inside from its
bookstores to robots used today by research institutions.
The development of the algorithm will provide maps of simulated environments from the data
provided by the sensors, in order that this work can be applied to autonomous robots is to work with
an Upboard board that is a small board computer for use developmental.
The results obtained show us that the algorithm achieves its task of giving autonomy to the robot by
generating a map and showing its location in the graphical environment and that it can work in the
embedded system (Upboard) to use more applications.
ÍNDICE GENERAL
1 .............................................................................................................................................. 10
INTRODUCCIÓN ................................................................................................................................ 10
1.1 ROBÓTICA............................................................................................................................................................ 10
1.2 MODELOS DE APRENDIZAJE DEL ENTORNO ........................................................................................................ 11
1.3 PLANTEAMIENTO DEL PRESENTE TRABAJO......................................................................................................... 11
1.4 ORGANIZACION DEL DOCUMENTO ..................................................................................................................... 12
2 ............................................................................................................................................... 13
FUNDAMENTACIÓN .......................................................................................................................... 13
2.1 ANTECEDENTES ................................................................................................................................................... 13
2.2 PLANTEAMIENTO DEL PROBLEMA ...................................................................................................................... 14
2.3 HIPOTESIS ............................................................................................................................................................ 15
2.4 OBJETIVOS ........................................................................................................................................................... 15
2.5 JUSTIFICACION .................................................................................................................................................... 16
2.6 LIMITACIONES ..................................................................................................................................................... 16
2.7 PRESUPUESTO ..................................................................................................................................................... 16
3 ............................................................................................................................................... 17
MARCO TEÓRICO .............................................................................................................................. 17
3.1 ROBOTS MÓVILES................................................................................................................................................ 17
3.2 SISTEMAS ODOMÉTRICOS ................................................................................................................................... 22
3.3 MAPAS DE NAVEGACIÓN Y SU APLICACIÓN AL SLAM ......................................................................................... 24
3.4 SEGUIMIENTO DE TRAYECTORIA Y EVASIÓN DE OBSTÁCULOS ........................................................................... 29
3.4 SENSORES ............................................................................................................................................................ 29
3.5 BOARD ................................................................................................................................................................. 33
3.6 ROBOT OPERATING SYSTEM ............................................................................................................................... 35
3.7 PROBABILIDADES ROBOTICAS ............................................................................................................................. 44
3.7.1 FILTRO DE BAYES .............................................................................................................................................. 45
3.7.2 FILTRO DE KALMAN (KF) ................................................................................................................................... 46
3.8 SLAM ................................................................................................................................................................... 50
3.9 NAVEGACIÓN ...................................................................................................................................................... 57
4 ............................................................................................................................................... 61
ROS .................................................................................................................................................. 61
4.1 INSTALACIÓN DE ROS .......................................................................................................................................... 61
4.2 INSTALACIÓN DE GAZEBO ................................................................................................................................... 62
4.3 ROBOT TURTLEBOT ............................................................................................................................................. 63
4.3 RVIZ ..................................................................................................................................................................... 68
4.4 GMAPPING .......................................................................................................................................................... 70
5 ............................................................................................................................................... 72
DESARROLLO DE ALGORITMO PARA ROBOT AUTÓNOMO .................................................................. 72
5.1 INSTALACIÓN Y PRUEBAS EN LA TARJETA UPBOARD .......................................................................................... 72
5.2 DESARROLLO DEL ALGORITMO ........................................................................................................................... 74
5.3 FILTROS DE BAYES ............................................................................................................................................... 75
5.4 IMPLEMENTACIÓN DE KF Y EKF .......................................................................................................................... 81
7
6 ............................................................................................................................................. 103
CONCLUSIONES Y TRABAJO A FUTURO .............................................................................................103
6.1 CONCLUSIONES ................................................................................................................................................. 103
6.2 ALCANCES................................................................................................................................................... 103
6.3 RECOMENDACIONES .................................................................................................................................. 104
A ............................................................................................................................................ 107
ANEXO A: CONCEPTOS BASICOS PARA COMPRENDER EL FILTRO DE KALMAN ....................................107
A.1. PROBABILIDAD DE UN EVENTO........................................................................................................................ 107
A.2.VARIABLES ALEATORIAS .................................................................................................................................... 108
A.3.MEDIA Y VARIANZA ........................................................................................................................................... 109
A.4.DISTRIBUCIÓN DE PROBABILIDAD NORMAL O GAUSSIANA ............................................................................. 110
A.5.INDEPENDENCIA Y PROBABILIDAD CONDICIONAL ........................................................................................... 111
B ............................................................................................................................................ 114
ANEXO B: DATASHEET ......................................................................................................................114
C ............................................................................................................................................. 117
ANEXO C:PROGRAMA GMAPPING.CPP .............................................................................................117
D ............................................................................................................................................ 123
ANEXO D: PROGRAMA ULTRASONIDO.INO .......................................................................................123
E ............................................................................................................................................. 125
ANEXO E: PROGRAMA ODOMETRIA Y SENSOR LIDAR........................................................................125
E.1 ODOMETRIA ...................................................................................................................................................... 125
E.2 SENSOR LIDAR ................................................................................................................................................... 131
8
ÍNDICE DE FIGURAS
Figura 1: Trayectoria de un robot móvil ........................................................................................................... 18
Figura 2: Robot rodante con cuatro ruedas...................................................................................................... 19
Figura 3: Robot velocista .................................................................................................................................. 19
Figura 4: Robot dotado de orugas .................................................................................................................... 20
Figura 5: Robot bípedo humanoide .................................................................................................................. 20
Figura 6: Robot reptador .................................................................................................................................. 21
Figura 7: Quadracpoptero ................................................................................................................................ 22
Figura 8: Avión espía no tripulado .................................................................................................................... 22
Figura 9: Cierre de bucle .................................................................................................................................. 25
Figura 10: Mapa topológico .............................................................................................................................. 26
Figura 11: Mapa de ocupación de celdillas ....................................................................................................... 27
Figura 12: Imágenes recibida por un sensor RPlidar ........................................................................................ 30
Figura 13: Sensor RPlidar .................................................................................................................................. 31
Figura 14: Modo de trabajo de un sensor RPlidar ............................................................................................ 32
Figura 15: Composición de un sensor Realsense .............................................................................................. 33
Figura 16: Raspberry pi ..................................................................................................................................... 34
Figura 17: Sistema embebido Upboard ............................................................................................................ 35
Figura 18: Modo de trabajo de Ros .................................................................................................................. 36
Figura 19: Comunicación entre dos sistemas con Ros ...................................................................................... 37
Figura 20: Suscripción directa del nodo de Display .......................................................................................... 38
Figura 21: Uso de servicios ............................................................................................................................... 38
Figura 22: Uso de la instrucción Rqt_graph para ver el modo de trabajos de los nodos ................................. 40
Figura 23: Grafico para desarrollo de la navegación de un Robot ................................................................... 42
Figura 24: Entorno del Software Rviz................................................................................................................ 43
Figura 25: Algoritmo de Bayes .......................................................................................................................... 46
Figura 26: Algoritmo de Kalman ....................................................................................................................... 48
Figura 27 Algoritmo EKF.................................................................................................................................... 50
Figura 28: Determinación de una línea mediante RANSAC .............................................................................. 54
Figura 29: Proceso de SLAM ............................................................................................................................. 56
Figura 30: Instalación de las librerías ROS ........................................................................................................ 61
Figura 31: Verificacion de instalacion de Gazebo ............................................................................................. 62
Figura 32: Entorno Gazebo. .............................................................................................................................. 63
Figura 33: Robot Turtlebot. .............................................................................................................................. 63
Figura 34: Comando para abrir el interfaz Gazebo con el robot Turtlebot. ..................................................... 64
Figura 35: Visualización del robot Turtlebot en Gazebo................................................................................... 64
Figura 36: Modo de trabajo de los modos con la instruccion rqt_graph. ........................................................ 65
Figura 37: Programa para determinar la distancia del robot Turtlebot hacia un objeto. ................................ 65
Figura 38: Imagen superior, muestra el valor de distancia entre el Robot y el objeto, en la imagen
intermedio se puede ver como varía la distancia y es proporcional a la distancia de cómo se aleja de este
como en la imagen inferior. .............................................................................................................................. 66
Figura 39: Programa para evitar obstáculos usando el sensor de distancia. ................................................... 67
Figura 40: Ventana de comando que muestra al ejecutar Rviz ........................................................................ 68
Figura 41: Visualización del robot Turtlebot en el entorno Gazebo ................................................................. 68
9
Figura 42: Visualización de los valores obtenidos por la cámara Kinect del robot Turtlebot .......................... 69
Figura 43: En la imagen superior se observa los objetos visualizados por el sensor que con la ayuda de la
ventana de teleoperacion se va desplazando alrededor como la imagen inferior. . ....................................... 69
Figura 44: Uso de la pauqete Gmapping para entornos ................................................................................... 70
Figura 45: Visualización actual de los objetos alrededor del Turtlebot ........................................................... 70
Figura 46: Se puede ver como se construye el mapa alredor de Turtlebot ..................................................... 71
Figura 47: Sistema embebido Upboard usado para realizar las pruebas. ........................................................ 73
Figura 48: Ventana de carga inicial en la Upboard. .......................................................................................... 73
Figura 49: Mensaje de instalacion correcta de los Kernel. ............................................................................... 73
Figura 50: Mensaje de instalacion de Ros exitosa. ........................................................................................... 74
Figura 51: Algoritmo para desarrollo de SLAM................................................................................................. 75
Figura 52: El ciclo de un filtro de Kalman en funcionamiento .......................................................................... 83
Figura 53: Algoritmo del filtro de Kalman......................................................................................................... 83
Figura 54: Funcionamiento del KF .................................................................................................................... 84
Figura 55: Algoritmo del EKF............................................................................................................................. 87
Figura 56: Valores recibidos por la odometría del robot. ................................................................................. 88
Figura 57: Valores recibidos por el sensor Kinet .............................................................................................. 89
Figura 58: Puntos de referencia para realizar la transformacion de la informacion ........................................ 89
Figura 59: Valores a ser enviados a los motores del robot Turtlebot .............................................................. 90
Figura 60: Mapa de costos ................................................................................................................................ 91
Figura 61: Diseño de entorno para pruebas ..................................................................................................... 91
Figura 62: Ventanas de visualizacion control y lectura de datos del Turtlebot................................................ 92
Figura 63: Ventanas de visualización control y lectura de datos del Turtlebot. ............................................... 93
Figura 64: Visualización de los predicciones a tomar el robot de color verde. ............................................... 93
Figura 65: La flecha verde indica la posición (meta) a dirigirse ........................................................................ 94
Figura 66: La flecha roja indica el camino a recorrer de forma autónoma ...................................................... 94
Figura 67: Se ve como el robot en el simulador se mueve de forma autónoma sin tocar los obstáculos ....... 95
Figura 68: Segundo entorno para pruebas ....................................................................................................... 95
Figura 69: Entorno listo para indicar metas...................................................................................................... 96
Figura 70: Evasión de obstáculo. ...................................................................................................................... 96
Figura 71 : Comunicaion entre la Upboard y el arduino. .................................................................................. 97
Figura 72 : Conexión entre el Arduino Mega y el sensor HC-SR04 ................................................................... 97
Figura 73: Ubicación de los dispositivos ........................................................................................................... 98
Figura 74 : Toma de distancias con el sensor ultrasonido. ............................................................................... 98
Figura 75 : Entorno para evitar obstaculos ....................................................................................................... 99
Figura 76 : Programa para evitar obstáculos .................................................................................................... 99
Figura 77 :Comunicacion del robot con la PC por medio de Wifi. .................................................................. 100
Figura 78: Conexionado del robot autonomo. ............................................................................................... 100
Figura 79: Diseño de robot autonomo para pruebas. .................................................................................... 101
Figura 80: Lectura del sensor laser scaner...................................................................................................... 101
Figura 81: Mapeo y contruccion del entorno. ................................................................................................ 102
10
1
INTRODUCCIÓN
________________________________________________________________________________________
1.1 ROBÓTICA
La robótica es una ciencia o rama de la tecnología, que estudia el diseño y construcción de máquinas
capaces de desempeñar tareas realizadas por el ser humano o que requieren del uso de inteligencia.
Las ciencias y tecnologías de las que deriva podrían ser: el álgebra, los autómatas programables, las
máquinas de estados, la mecánica o la informática.
Los robots se han vuelto cada vez más presentes en la vida diaria. Se pueden ver en las casas
aspirando el piso, cortando el pasto o incluso enfriando los alimentos a la temperatura correcta. Los
llamados robots de servicio, robots domésticos, robots de ayuda a discapacitados y robots de ayuda
en general forman parte de un área que crece a ritmo acelerado y que en unos años será posible verlos
cocinando diversos platillos o llevando a sus dueños al trabajo.
Los grandes avances en el diseño y construcción de los robots los han hecho herramientas cada vez
más útiles en todas las áreas del quehacer humano. Sobre todo, han demostrado su valía
desarrollando tareas muy repetitivas y tediosas, como rellenar botellas de agua en una envasadora y
tareas muy pesadas para los humanos como ensamblar las partes de un automóvil en una línea de
11
producción; también han aumentado el alcance y precisión para muchas tareas como en aplicaciones
médicas o incluso realizando tareas muy peligrosas como manipulación de explosivos o elementos
radioactivos.
Uno de los principales problemas es que nos encontramos en un mundo variante, donde las cosas
cambian rápidamente es por eso que hay que buscar que los robots sean capaces de detectar estas
variaciones y puedan tomar decisiones, es por eso
Para que un robot pueda realizar el aprendizaje de un mapa este requiere de tres tareas que son: El
modelado, la localización y la planificación de la ruta.
En el segundo capítulo se fundamenta la realización del trabajo, donde se detalla el planteamiento del
problema, se formula la hipótesis, se fijan los objetivos y se da la justificación debida para el
desarrollo del problema planteado así como la limitación del trabajo.
En el tercer capítulo se dan los fundamentos teóricos para el desarrollo del proyecto, como son los
robots móviles, los sistemas odómetricos, los mapas de navegación y sus aplicaciones, sensores,
board, robot operating system, probabilidades robóticas, filtro de Bayes y de Kalman.
En el cuarto capítulo se habla sobre la instalación de ROS y el software a ser utilizados para la
realización de las pruebas, así como el tipo de robot en el cual se realizaran las pruebas.
En el quinto capítulo se desarrolla el algoritmo para el robot el cual comienza con la instalación de
las librerías ROS en la placa de desarrollo Intel, para la simulación y la implementación de un robot
real para realizar las pruebas.
2
FUNDAMENTACIÓN
________________________________________________________________________________________
2.1 ANTECEDENTES
Hoy en día en la industria de vehículos móviles está agregando cada vez más sistemas avanzados
para tener autonomía en la hora del manejo, una de estos es aplicada en los autos donde se requiere la
asistencia al conductor progresando hacia un vehículo totalmente autónomo. Según encuesta de los
últimos años se ve que cada año hay alrededor de 1,2 millones de muertos en accidentes de tráfico, de
los cuales el 90% son causados por errores humanos [1]. El uso de algoritmos de control puede
reducir el consumo de combustible en un 4-10% acelerando y desacelerar más suavemente que un
conductor humano y disminuir significativamente
Al tener una computadora en control del vehículo, el conductor es capaz de hacer otras
actividades productivas o agradables como trabajar, leer, ver una película o incluso dormir durante el
viaje. Para contar con un sistema de control una estimación de posición fiable y un mapa del medio
ambiente son cruciales.
Un dilema en la robótica móvil es que para construir un mapa y conocer, la posición exacta
del robot, la estimación es necesaria, pero para conocer la posición del robot se requiere un mapa.
Hoy en día el SLAM puede ser aplicado en los robots, estos tienen las características de
operar en entornos completamente impredecibles y ser afectados de forma incremental por la lectura
14
de los sensores. Para este fin es necesario desarrollar un software robusto que permita a los robots
soportar los numerosos desafíos que surgen en entornos no estructurados y dinámicos.
Una tecnología de detección del medio ambiente, comúnmente utilizada para la detección de
obstáculos, es detección de luz y Ranger (LIDAR) [15], que estima la distancia a un objeto a través
de un dispositivo LIDAR basado en el retardo de tiempo entre un pulso de luz y la detección de la
Reflexión.
Mientras que para realizar una detección mayor existe el sensor de Kinect [14], el cual está
diseñado para estar en una posición longitudinal. El dispositivo tiene una cámara RGB, sensor de
profundidad y un micrófono multi-array bidireccional que conjuntamente capturan el movimiento de
los cuerpos en 3D, además de ofrecer reconocimiento facial y aceptar comandos de voz.
Los robots móviles a diferencia de los robots convencionales, manejan espacios de trabajo
mayores gracias a la capacidad que poseen de moverse. Consecuencia de esta capacidad es que los
robots móviles necesitan enfrentar diversos tipos de entornos. A fin de mejorar la performance del
control del robot, diversos sensores han sido integrados al robot: sensores de ultrasonido, sonar,
cámaras, GPS, etc. De entre ellos los sensores laser y cámaras son una alternativa bastante práctica
al momento de obtener información del entorno, pues recupera gran cantidad de información del
entorno. Usando la realimentación visual se tiene un gran potencial para incrementar la flexibilidad
del sistema a fin de enfrentar entornos cambiantes o poco estructurados. Es bien sabido que la
implementación de este tipo de sensores en el robot y la realimentación de sus señales, mejora el
control de su desplazamiento dándole una forma de autonomía al momento de enfrentar eventos
presentes en su movimiento, conocidos estos como Robots Autónomos.
Frente a ello nace la pregunta si es posible que un robot móvil sea colocado en un entorno
desconocido y que éste construya de forma incremental un mapa de este entorno y al mismo tiempo
determinar su ubicación. Ante esta situación es que se usa el SLAM (Simultaneous Localization and
Mapping) método para construir un mapa del entorno y al mismo tiempo deducir su ubicación [3].
Por lo antes expuesto, en este contexto, es que es necesario desarrollar nuevos algoritmos
basados en información de los sensores que realicen una integración de métodos para reconocer
15
objetos y determinar una ubicación, con el propósito de mejorar la autonomía de los robots móviles
[5]. De esta forma, se pretende desarrollar un algoritmo que se pueda aplicar al robot autónomo para
navegación en exteriores capaz de tomar decisiones para resolver un problema de búsqueda y/o
ubicación.
2.3 HIPOTESIS
El avance tecnológico permite que los desarrolladores puedan realizar una diversidad de
proyectos en el área de Robótica y Autonomía gracias a sistemas de hardware y software abierto para
su completo desarrollo, por lo que es posible la simulación e implmentacion de un prototipo de robot
móvil autónomo capaz de analizar el entorno y tomar decisiones utilizando técnicas de SLAM
usando como procesador una placa embebida Intel.
2.4 OBJETIVOS
Diseño de un algoritmo basado en SLAM aplicado en un robot móvil que pueda ser capaz de
reconocer objetos y construir un mapa del entorno, basado en una placa embebida para desarrollo de
hardware libre.
Desarrollar un algoritmo que con ayuda de los sensores pueda realizar la construcción de
mapas del entorno basado en SLAM.
Probar los algoritmos en un simulador de entornos.
Implementar la lógica en una placa embebida para simular los sensores que permitan obtener
valores de localización, objetos cercanos y cámara para adquisición de imágenes este
16
2.5 JUSTIFICACION
La finalidad es llegar a tener un algoritmo que se pueda aplicar a un robot interactivo capaz
de navegar, razonar e interactuar en entornos hostiles, para llegar a realizar estas tareas, el robot
debe ser capaz de adquirir todo tipo de información a través de su sistema sensorial.
2.6 LIMITACIONES
Una de las principales limitaciones será la información referente a aplicar los algoritmos de
autonomía utilizando el SLAM con el cual determinar el área donde se encuentra.
2.7 PRESUPUESTO
3
MARCO TEÓRICO
________________________________________________________________________________________
Una definición de robot móvil viene de la teoría clásica de robots [1]: “Un robot móvil es un
vehículo de propulsión autónoma y movimiento (re) programado por medio de control automático
para realizar una tarea específica.”
Los robots móviles se clasifican en guiados y no guiados. El vehículo guiado está restringido
a un conjunto de trayectorias predefinidas en su área de trabajo. Estas trayectorias están indicadas
por líneas ópticas o magnéticas o una secuencia de movimientos guardados en la memoria. El robot
en ningún momento puede abandonar la trayectoria. A estos robots se les llama también vehículos
automáticos guiados.
Los vehículos no-guiados no están restringidos a una trayectoria predefinida. A este tipo de
robots no-guiados pertenecen los robots submarinos (bajo el agua), los del espacio aéreo y/o
espacial y los terrestres. Los robots terrestres han sido clasificados por su sistema de movimiento en
robot con ruedas, con patas o con rieles. Así cada robot desarrolla diferentes tareas de acuerdo a su
capacidad. El problema básico de un robot móvil es el de navegación, es decir, moverse de un lugar
a otro auxiliado de sensores, planeación y control. El problema de navegación (Figura 1) es
simplemente encontrar una trayectoria desde un inicio (O) a una meta (M).
18
Para que su movimiento sea eficiente y autónomo requiere de sensores y actuadores que le
permitan tener información del mundo real. También requiere de una buena interface que le permita
recibir los estímulos de forma óptima y generar la respuesta adecuada para el desarrollo de su tarea.
Teniendo esta capacidad el robot puede planear sus movimientos y ejecutarlos de manera eficiente
teniendo un conjunto de algoritmos especiales. Esto requiere del conocimiento de la cinemática que
nos dará el conjunto de movimientos posibles del robot y de la dinámica la cual nos dará el conjunto
de fuerzas que se requieren para obtener el movimiento apropiado para cumplir el objetivo de la
tarea.
Existen en el mercado una serie de robots diseñados para tareas y aplicaciones específicas.
Esta especialización puede llegar a ser un inconveniente cuando se desea añadir una tarea o grupo de
tareas diferentes a la que fue diseñado, ya que con frecuencia los comerciantes o diseñadores solo
facilitan la información básica del robot referente a la tarea específica. Otro detalle más es el costo
elevado que estos prototipos tienen.
Atendiendo a sus capacidades de movimiento, podemos clasificar los robots móviles en las
siguientes categorías:
Robots rodantes: Son aquellos que, como su nombre indica, se desplazan haciendo uso de
ruedas, generalmente montadas por pares en una configuración 2+2 como las de un vehículo por
19
mera simplicidad. Habitualmente solo dos de sus ruedas presentan tracción y otras dos direcciones
(Figura 2), de forma que sea posible maniobrar el robot con un solo servomotor [26].
Existen algunos casos especiales en los que se usan otras configuraciones que dotan al robot
de mejor adaptación a terrenos difíciles. En estos casos los algoritmos de control de movimiento
adquieren una mayor complejidad, proporcional al número de elementos direccionales de forma
independiente. Por ultimo cabria considerar a los robots con orugas como un tipo de robot rodante
en el que se substituyen las ruedas por un mecanismo de oruga para la tracción (Figura 4). La
20
dirección se consigue parando una de las orugas o haciéndolas girar en sentido contrario este
modelo de robots son muy utilizados en zonas de relieve rocoso.
Robots andantes: Respecto a los robots construidos a imagen y semejanza humana, con
dos piernas, las técnicas de control necesarias son varias, pero todas ellas hacen uso de complejos
algoritmos para poder mantener el equilibrio y caminar correctamente ya que se ayudan de sensores
como giroscopio y acelerómetro. Todos ellos son capaces de caminar bien sobre suelos planos y
subir escaleras en algunos casos, pero no están preparados para caminar en suelos irregulares
(Figura 5). Algunos incluso pueden realizar tareas como bailar, luchar o practicar deportes, pero
esto requiere una programación lo cual ya muchas Startup han liberado información para que
muchos desarrolladores puedan acceder a estas librerías y mejorarlas pero va de la mano con
hardware del robot y de su capacidad de procesamiento.
Robots reptadores: Una clase curiosa de robots, creados basándose en animales como las
serpientes, su forma de desplazarse es también una imitación de la usada por estos animales. Están
formados por un número elevado de secciones que pueden cambiar de tamaño o posición de forma
21
independiente de las demás pero coordinada (Figura 6), de forma que en conjunto provoquen el
desplazamiento del robot [26].
Figura 7: Quadracpoptero
La odometría es una técnica que tiene por objeto estimar la posición y orientación de un
vehículo a partir del número de vueltas dadas por sus ruedas [2]. La odometría se utiliza para
estimar la posición relativa de un robot móvil. En nuestro caso, nos interesa la posición del robot en
el plano, es decir, nos interesa estimar el vector (𝑥, 𝑦, 𝜃).
Hablamos de estimación de posición ya que saber a ciencia cierta la posición de un robot
móvil es sencillamente imposible. Por qué los métodos que utilizamos para calcular esa posición no
tienen una precisión absoluta. Nuestra única forma de obtener información de un robot móvil y su
23
entorno es a través de los sensores. Sobre la información recibida por los sensores se tratara de
reducir el error realizando comparaciones aun así estas medidas nunca se librarán de errores. Cómo
no, los errores de los sensores serán inversamente proporcionales al precio de los mismos. Para ello
se emplean codificadores ópticos de elevada precisión montados sobre los ejes de las ruedas que
permiten llevar una cuenta bastante precisa del número de vueltas (y fracción) que estas realizan.
Para la estimación se requiere el registro odómetrico de al menos dos ruedas del vehículo [3].
Una clara distinción entre errores sistemáticos y no sistemáticos es de gran importancia a la hora
de reducir los errores en odometría. Por ejemplo, los errores sistemáticos se pueden corregir con una
buena calibración, aunque en superficies no rugosas de entornos interiores son éstos los que
contribuyen más a los errores de odometría que los no sistemáticos. En cambio, en superficies
abruptas con irregularidades significativas, son los no sistemáticos los que predominan.
24
eficiente para operar en tiempo real, por otro lado es aconsejable que la asociación sea robusta para
asegurar el buen funcionamiento en caso de trabajar en espacios grandes o en situaciones en las que
no es necesario explorar todo el entorno pues una parte de él ya ha sido previamente modelado y
sólo existe una pequeña región sin explorar.
Detección de un ciclo: Nos referimos a la detección de ciclo, cuando un robot que está
realizando la exploración de un entorno es capaz de identificar que vuelve a una región del mapa ya
explorada y que por tanto ha cerrado el bucle de su trayectoria (Figura 9).
Este aspecto está muy ligado al concepto descrito en el párrafo anterior de la asociación de
datos, la eficiencia de búsqueda es un aspecto importante pero aún más lo es la robustez en la
decisión de si una asociación es correcta o no. Una vez encontrada una asociación correcta, el error
acumulado en el mapa tiene que ser compensado correctamente cada vez que se actualice el mapa.
Tiempo de computación almacenado: El mapa tiene que almacenar suficiente información
para permitir asociación de datos y convergencia. Este almacenaje y el tiempo de computación
tienen que ser proporcionales al área tratada.
Los mapas topológicos usando los grafos representan las características del entorno por el
robot móvil. Los nodos representan lugares distintivos del entorno, constituyendo la unidad
elemental de localización, y los arcos caminos a ser recorridos por el robot entre los lugares. Los
26
lugares representativos del entorno (landmarks) son seleccionados en base a sus características
distinguibles de forma unívoca.
El SLAM topológico explora el entorno siguiendo una serie de criterios con los que
determina la trayectoria, al mismo tiempo va guardando las descripciones de los lugares. Cada vez
que un lugar nuevo es encontrado, es conectado al lugar anterior siguiendo las especificaciones
hasta llegar a él. De esta manera, el mapa es construido como secuencia lineal de lugares que
continúa hasta que un lugar encontrado cuadre con uno anteriormente almacenado. Si este lugar
puede ser catalogado como uno anterior entonces se cierra el ciclo o bucle.
Uno de los problemas más importantes que presenta esta técnica es la asociación de datos. En caso
de detección de un ciclo en un lugar que coincida con uno anteriormente almacenado, la asociación
de datos pasa a ser ambigua y el lugar observado puede ser uno de los previamente registrados o uno
nuevo que se asemeje, distinguir entre estos dos casos no siempre es fácil. Una posible solución
sería introducir algún tipo de información métrica.
Un inconveniente de los mapas topológicos es que no se pueden representar zonas abiertas
(habitaciones grandes, halls), debido a que el alcance limitado de los sensores no obtiene
información del entorno (Figura 10). Otro inconveniente es que los mapas construidos dependen
excesivamente de la historia de las percepciones del robot al construirlos [12].
Siguiendo el ejemplo de la (Figura 10), el arco entre P1 y P2 está etiquetado como seguir la
pared izquierda porque esa es la conducta local que el robot siguió para ir de un nodo a otro. Esto
hace que los mapas topológicos sean muy sensibles a la aparición de elementos no modelados
27
La técnica de Occupancy grid intercala los pasos de localización y actualización del mapa,
primero registrando el mapa a corto plazo y luego fundiéndolo con el mapa global (localización),
para más tarde actualizar la ocupación percibida del mapa global (mapeo). Este método funciona
muy bien para entornos dinámicos y pequeños en periodos breves de tiempo, sin embargo no ofrece
un modelo exacto de incertidumbre por lo que en periodos muy largos tiende a divergir [16].
Algunas de las ventajas que ofrece esta solución es que se trata de un algoritmo robusto y de
implementación sencilla, además es capaz de distinguir entre zonas ocupadas y vacías, consiguiendo
una partición y descripción completa del espacio explorado. Por este motivo es popular en tareas de
28
Uno de los problemas que presenta este tipo de mapas hace referencia a la asociación de
datos. La correcta estimación de la pose depende de encontrar una correcta correspondencia entre el
punto característico observado y el asociado al mapa. Una asociación errónea daría como resultado
una reducción de la incertidumbre sobre la posición del robot y el error estimado aumentaría. Cabe
decir, que estos mapas son sólo recomendables para entornos donde los objetos pueden ser
fácilmente descritos por un modelo geométrico.
29
En lo que respecta a la evasión de obstáculos, existen distintos métodos para llevar a cabo
esta tarea. Los más relevantes son: por detección de bordes [5] y por construcción de mapas [6]. En
el método por detección de bordes [5], el algoritmo implementado, permite que el robot detecte los
bordes verticales de un posible obstáculo; sin embargo, dependiendo de los sensores que hagan esta
detección, es necesario que el robot se detenga cuando detecte dicho borde y compute la
información necesaria para confirmar la presencia del obstáculo. En el método por construcción de
mapas [6], el algoritmo implementado permite crear un grafo que conecta cada punto del espacio
libre de trabajo, facilitando la generación de un camino que permita al robot navegar desde su punto
inicial a su punto final eliminando la incertidumbre de posibles obstáculos.
3.4 SENSORES
Hoy en día para poder tomar las medidas del entorno que le rodea a un robots es que usan los
sensores el cual existen para todo tipo de aplicación claro está que estos van de la mano con el
precio, a mayor sea la exactitud de las medidas mayor será el precio de estas, para lo cual se detalla
los sensores que se podrían utilizar para poder desarrollar el SLAM.
30
Los sistemas LiDAR topográficos usan dos aproximaciones para entregar las mediciones de
distancias. La primera mide el tiempo de ida y vuelta para un pulso de luz desde el laser
hasta objetivo y de vuelta al laser, y la segunda lleva a cabo la medición a través de la
diferencia de fase entre la señal transmitida y recibida (Figura 12).
Durante los últimos años una nueva generación de scanners ha sido desarrollada, los cuales
son capaces de registrar la señal completa del retorno de un pulso laser. Estos nuevos sensores son
denominados LiDAR de retorno de onda completa (Fullwaveform).
Se podría entender entonces que los sistemas discretos identifican con el retorno de la señal
las alturas de los picos, mientras que los waveform registran la huella completa de la señal para
procesarla posteriormente.
Dos nuevas contribuciones pueden ser hechas por los sistemas de retorno de onda completa:
Las potencialidades de los sensores de retorno de onda completa son numerosas, y en los
próximos años serán importantes para el desarrollo de nuevos métodos y programas. Entre las
31
aplicaciones está la posibilidad de mejorar la caracterización de la estructura del dosel sobre áreas
extensas y la disponibilidad de datos a nivel global.
RPLIDAR es una solución de escáner láser 2D de 360 grados desarrollada por RoboPeak. El
sistema puede realizar una exploración de 360 grados dentro de un rango de 6 metros. Los datos de
nube de puntos 2D producidos pueden usarse en mapeo, localización y modelado de objeto /
entorno.
La frecuencia de barrido de RPLIDAR alcanzó 5.5 Hz cuando se tomaron 360 puntos por
ronda.
RPLIDAR emite una señal láser infrarroja modulada y la señal láser es reflejada por el
objeto a detectar (Figura 13). La señal de retorno es muestreada por el sistema de adquisición de
visión en RPLIDAR y el DSP incorporado en RPLIDAR comienza a procesar los datos de muestra y
el valor de distancia de salida y el valor de ángulo entre objeto y RPLIDAR a través de la interfaz de
comunicación.
La cámara R200 es un dispositivo USB 3.0 que puede proporcionar corrientes de vídeo en
color, profundidad e infrarrojos. Los flujos de vídeo de profundidad son como flujos de vídeo en
color, excepto que cada píxel tiene un valor que representa la distancia de la cámara en lugar de la
información de color. Se compone de un sistema de proyección de infrarrojos láser, dos infrarrojos y
un full HD sensores de imagen en color. El flujo de vídeo de profundidad se genera con tecnología
de visión estéreo asistida por el proyector de láser infrarrojo y los dos sensores de imágenes
infrarrojas (Figura 15). Los datos de color son proporcionados por el sensor de imágenes en color
Full HD. El módulo R200 no pretende ser la solución fotográfica principal. Tiene la capacidad de
sincronizar con una cámara de alta resolución frente al mundo para aplicaciones de profundidad y
fotografía
33
3.5 BOARD
3.5.1 Raspberry pi
A esta placa de puede conectar monitor, ratón y teclado y ayudar a personas de todas las
edades a adentrarse en el mundo de la computación y la programación. La idea de desarrollar algo
así surgió en 2006 cuando Eben Upton, Rob Mullins, Jack Lang and Alan Mycroft del laboratorio de
informática de la Universdad de Cambridge empezaron a ver cómo había cambiado los
conocimientos de los niños sobre la informática. En la década de 1990 la mayoría de los niños
tenían mucha experiencia como programadores aficionados, en cambio en la década del 2000 solo
eran capaces de realizar diseño web.
mentalidad y educándolos en la ética del “ábrelo y mira cómo funciona”. En agosto de 2011, se
fabricaron cincuenta placas Alpha, que tenían las mismas características que el modelo B, pero eran
un poco más grandes para integrar bien unas interfaces para depuración. Las primeras ventas del
modelo B tuvieron lugar el 29 de febrero de 2012. El 4 de febrero de 2013, se lanzó el modelo A,
que venía con solo 256Mb de RAM y sin puerto ethernet a un precio más asequible que el modelo B
(Figura 16).
Hoy en día la Raspberry Pi presenta cuatro modelos, los modelos A+ y B+, que están
basados en sus predecesores A y B, la Raspberry Pi 2 modelo B y su último modelo Raspberry pi 3.
Las principales diferencias entre los modelos antiguos A y B y sus versiones más recientes A+ y B+
respectivamente están en el almacenamiento, pasando de ser SD a microSD y en el caso del modelo
A una reducción de tamaño y por tanto de peso. Otra diferencia del modelo B es el aumento de 2 a 4
puertos USB. Mientras que con las versión 3 este ya incluye Wifi y mejor procesador.
3.5.2 Upboard
de las cosas. La compatibilidad con Linux, Android y todas las distribuciones de Windows 10 le
brindan gran flexibilidad, escalabilidad.
Como resultado, ROS fue construido desde cero para fomentar el desarrollo de software de
robótica colaborativa. Por ejemplo, un laboratorio podría tener expertos en el mapeo de ambientes
interiores, y podría contribuir con un sistema de clase mundial para producir mapas. Otro grupo
podría tener expertos en el uso de mapas para navegar, y otro grupo podría haber descubierto un
enfoque de visión por computadora que funciona bien para reconocer objetos pequeños en el
36
desorden. ROS fue diseñado específicamente para grupos como estos para colaborar y construir
sobre el trabajo de cada uno, como se describe a lo largo de este sitio.
Entonces ROS (Robot Operating System) es un sistema con licencia BSD para controlar
componentes robóticos desde una PC. Un sistema ROS está compuesto por una serie de nodos
independientes, cada uno de los cuales se comunica con los otros nodos utilizando un modelo de
publicación / suscripción de mensajes. Por ejemplo, un controlador de un sensor en particular puede
ser implementado como un nodo, que publica datos de sensores en una corriente de mensajes. Estos
mensajes podrían ser consumidos por cualquier número de otros nodos, incluyendo filtros y
registradores.
Todos estos factores son una garantía debido a que Standford es una de las cunas de la informática y
la robótica, donde han nacido empresas tan importantes como Apple y Google.
ROS comienza con el ROS Master. El Maestro permite que todas las otras piezas de software ROS
(Nodos) encuentren y hablen entre sí. Simplemente podemos decirle al nodo 1 que envíe mensajes al
nodo 2 (Figura 18).
¿Cómo lo hacen los nodos? Al publicar y suscribirse a Temas, digamos que tenemos una
cámara en nuestro Robot. Queremos poder ver las imágenes de la cámara, tanto en el Robot en sí,
como en otro portátil.
Al registrarse con el ROS Master, el nodo de cámara indica que publicará un tema llamado /
image_data (por ejemplo). Los otros nodos registran que se suscriben al tema / image_data (Figura
20)
Por lo tanto, una vez que el nodo de cámara recibe algunos datos de la cámara, envía el
mensaje / image_data directamente a los otros dos nodos. (A través de lo que es esencialmente TCP
/ IP)
38
Ahora si se quisiera que el nodo de procesamiento de imágenes solicite datos desde el nodo
de la cámara en un momento específico, ROS implementa los: Servicios.
Un Nodo puede registrar un servicio específico con el ROS Master, así como registra sus
mensajes. En el siguiente ejemplo, el nodo de procesamiento de imágenes primero solicita /
image_data, el nodo de cámara recopila datos de la cámara y, a continuación, envía la respuesta
(Figura 21).
a) Red punto a punto: Un sistema basado en ROS consiste en un número de procesos, con
posibilidades de diferentes anfitriones conectados en tiempo real en una topología de red punto a
punto.
b) Multilenguaje: ROS ha sido diseñado para soportar diferentes lenguajes de programación (C++,
Python, Octave y LISP principalmente). Para poder realizar esta tarea, se emplea un lenguaje neutral
y se hace uso de una interfaz de definición de lenguaje (IDL - Interface Definition Language) para
establecer los mensajes que serán empleados para la comunicación entre módulos. De esta manera, a
partir del lenguaje neutral se produce una generación de código, de forma automática, en los otros
lenguajes y de esta manera poder enviar y recibir mensajes, información, entre módulos escritos en
el mismo estilo de lenguaje como los que no lo están.
c) Herramientas: Para mejorar el manejo de la complejidad de ROS se cuenta con una gran variedad
de mini herramientas, utilizadas para generar los diversos componentes de ROS. Estas ejecutan
varias tareas como: configuración de parámetros, visualización y monitoreo de la conexión punto a
punto, manejo de la información de los mensajes, entre otras. La separación de las tareas en
módulos puede acarrear en una pérdida de la eficiencia pero se gana un manejo más estable.
d) Ligero: Los drivers o algoritmos generados para la operatividad de los múltiples robots existentes
podrían ser reutilizados o reciclados fuera del proyecto en el que se encuentran. En este sentido, para
asegurar esta filosofía el sistema de construcción de ROS se produce en el código fuente y se
emplea CMake, para lograr que la complejidad caiga en las librerías y que se creen ejecutables
asociados a esas librerías funcionales de ROS.
e) Software libre: El código fuente de ROS es público y se encuentra disponible. Es distribuido bajo
la licencia BSD lo que permite el desarrollado de proyectos comerciales. Por otra parte, el uso de
ínter procesos de comunicación no requiere que los módulos enlazados estén contenidos en el
mismo ejecutable, hace que se pueda realizar pases de información entre módulos. De esta manera,
todos los sistemas construidos alrededor de ROS pueden hacer uso de varios tipos de licencia, como
GLP (General Public License) y BSD.
40
3.6.2 Fundamentos
Las bases operacionales en las que se encuentra implementado ROS son: gerencia de nodos,
mensajes, tópicos y relaciones cliente-servidor.
a) Nodos: La premisa en la que es desarrollado ROS es en que sea modular. En este sentido, el
término nodo se encuentra referenciado a un módulo de software, un programa. Las relaciones que
se establecen entre los nodos pueden ser visualizados con la ayuda de la herramienta gráfica
RXGRAPH, en ella los nodos aparecen encerrados en círculos.
b) Mensajes: La comunicación entre los nodos se realiza por medio de mensajes. Estos son un tipo
estructurado de datos, en varios formatos, como: enteros, punto flotante, booleano. Por otra parte, un
mensaje puede estar definido por otro mensaje o conjunto de mensajes. La conexión entre los nodos
en la herramienta RXGRAPH aparece representa por una flecha, y su sentido indica que nodo está
enviando el mensaje y que nodo lo está recibiendo.
c) Tópico: La comunicación de un nodo puede ser de escuchar o de hablar, y esto es posible por
medio de los tópicos, en que los mensajes son publicados para que cualquier nodo pueda extraer la
información por medio de una suscripción, en la herramienta Rqt_graph los tópicos son colocados
encima de las flechas que representa la conexión entre nodos, ver (Figura 22)
d) Cliente – Servidor: Es un sistema de comunicación definida por una llamada a servicio, en la que
se realiza una petición (cliente) y se da una respuesta (servidor). En este modelo también se
encuentra definido un mensaje.
Figura 22: Uso de la instrucción Rqt_graph para ver el modo de trabajos de los nodos
41
El primer paso para poder iniciar una investigación en el ámbito de la robótica debe consistir
en definir los límites del sistema en el que vamos a trabajar, de esta manera nos podremos centrar
por ejemplo en un nodo que realiza localización, o un nodo que se encarga de la percepción o del
control. Sin embargo, para poner en funcionamiento el sistema robótico es necesario que exista un
ecosistema de software que una de manera adecuada cada parte individual del sistema.
Los nodos están conectados de forma dinámica en tiempo de ejecución, pero sólo se
reinician periódicamente aquellos nodos que necesitan modificar su código fuente, siendo el propio
sistema quien se encarga de la modificación. Es por esto que ROS está diseñado para minimizar la
depuración en casos en los que nuestra aréa de estudio forme parte de un gran sistema, de esta forma
se consigue un aumento de la productividad y un sistema más complejo e interconectado.
3.6.3.2 Registro y Reproducción
Cualquier flujo de mensajes de ROS puede ser almacenado en el disco y reproducirse más
tarde. Todo esto puede hacerse en la línea de comandos y no requiere la modificación del código
fuente de ninguna de las piezas software. Para facilitar el registro y seguimiento de los sistemas
distribuidos a través de muchas máquinas, la librería rosconsole proporciona una interfaz en la cual
todos los registros se publican en el topic rosout.
3.6.3.4 Subsistemas
Localizacion
Planificador
Cada uno de los nodos anteriores puede ser ejecutado en una línea de comandos con su
instrucción correspondiente, pero esta labor puede ser muy tediosa. Para solventar este problema,
existe una herramienta en ROS llamada roslaunch que permite de forma fácil poner en marcha
múltiples nodos de ROS a nivel local y remoto a través de SSH, así como los parámetros de
configuración en el servidor de parámetros.
Los roslaunch están definidos como archivos de configuración XML (con la extensión de su
lanzamiento), en los cuales se especifican los parámetros necesarios para establecer y poner en
marcha los nodos, además de indicar las máquinas en que deben ejecutarse. Esta funcionalidad ayuda
de manera significativa no sólo al intercambio y a la reutilización, sino también a la integración,
instalación y desmontaje de grandes sistemas distribuidos.
Esta herramienta nos ha sido de gran utilidad y nos ha simplificado el trabajo ya que en el
problema SLAM intervienen multitud de nodos, cada uno de los cuales necesita ser ejecutado en un
terminal distinto. Creándonos un archivo .launch hemos conseguido ejecutar todos ellos en un único
terminal y al mismo tiempo poner en funcionamiento al nodo maestro que a su vez comienza a
comunicar las ordenes necesarias a todos los nodos implicados.
Un paquete en ROS no es más que un directorio que contiene un archivo XML en el que
encontramos una descripción del mismo y de sus dependencias.
El sistema de archivos se organiza de la siguiente manera:
a) Packages: Son la unidad principal en la que se organiza el software en ROS. Un paquete puede
contener procesos ejecutables (nodos), librerías dependientes de ROS, archivos de configuración etc.
b) Manifest: Proporcionan información sobre un paquete, incluyendo tipo de licencia de uso y
publicación, sus dependencias de otros paquetes e información específica de compilación.
c) Stacks: Son conjuntos de paquetes que proporcionan una funcionalidad concreta, es la manera en
la que el software es publicado contando con números de versión asociados.
Durante la etapa de diseño y depuración del software se hace necesario observar distintos estados
mientras el sistema se está ejecutando. ROS es capaz de acceder al sistema gráfico y observar estos
estados mediante un visualizador para tal fin llamado Rviz (Figura 24).
La herramienta Rviz permite visualizar el funcionamiento del algoritmo, además resulta muy
atractiva para mostrar cómo se va modelando un mapa del entorno.
Aunque tiene la ventaja de ser muy potente, posee su limitación pues considerado que el
computador portátil en el que se comenzó a desarrollar el proyecto no tenía la potencia necesaria para
soportar esta herramienta siendo necesario recurrir una placa embebida con las características
adecuadas.
44
Existen otras herramientas de línea de comandos que nos permiten visualizar un gráfico de
cómputo de ROS, es el caso de rxgraph que nos muestra los nodos que se están ejecutando
(representados por óvalos), así como los topic que los conectan (arcos).
La característica más llamativa de los nuevos sistemas de robots [8] es que operan en
entornos cada vez más desestructurados, ambientes que son inherentemente impredecibles. Como
resultado, la robótica se está moviendo hacia áreas donde la entrada de sensores es cada vez más
importante, y donde el software de robot tiene que ser lo suficientemente robusto como para hacer
frente a una serie de situaciones, muchas veces demasiado para anticiparlas a todas. La robótica, por
lo tanto, se está convirtiendo cada vez más en una ciencia del software, donde el objetivo es
desarrollar un software robusto que permita a los robots resistir los numerosos desafíos que surgen en
entornos no estructurados y dinámicos.
1. Ambientes: Los mundos físicos son intrínsecamente impredecibles. Mientras que el grado de
incertidumbre en entornos bien estructurados es pequeña, en ambientes tales como autopistas
y casas particulares son altamente dinámicas e impredecibles.
2. Sensores: Los sensores están intrínsecamente limitados en lo que pueden percibir. Donde
podemos observar dos factores primarios. Primero, el alcance y la resolución de un sensor
están sujetos a las leyes físicas. Por ejemplo, las cámaras no pueden ver a través de las
paredes, e incluso dentro del rango perceptual la resolución espacial de las imágenes de la
cámara es limitada. En segundo lugar, los sensores están sujetos a ruido, lo que perturba las
mediciones del sensor en condiciones impredecibles y por lo tanto limita la información que
se puede extraer de mediciones del sensor.
3. Robots: La actuación del robot implica motores que son, al menos hasta cierto punto,
impredecibles, debido a los efectos como el ruido de control y desgaste y rasgón. Algunos
actuadores, como brazos robustos de robots industriales, son bastante precisos. Otros, como
los de bajo costo robots móviles, pueden ser extremadamente inexactos.
45
4. Modelos: Los modelos son intrínsecamente inexactos. Los modelos son abstracciones del real
mundo. Como tal, sólo modelan parcialmente los procesos físicos subyacentes del robot y su
entorno. Los errores de modelo son una fuente de incertidumbre que en la robótica, a pesar de
que la mayoría de los modelos robóticos utilizados en los sistemas de robótica de última
generación son bastante toscos.
5. Computación: Los robots son sistemas en tiempo real, lo que limita la cantidad de
computación que puede llevarse a cabo. Muchos algoritmos de vanguardia son aproximados,
logrando respuesta oportuna pero sacrificando la precisión.
La manera evidente de hacerlo es considerar que tanto la posición del robot como los
elementos que modelan su entorno son variables aleatorias. Así, los algoritmos existentes modelan
ambos de manera probabilística, y utilizan métodos de inferencia para determinar aquella
configuración que es más probable teniendo en cuenta las medidas que se van obteniendo.
El principio básico que subyace en cualquier solución exitosa del SLAM es la regla de
Bayes. Para dos variables aleatorias, a y b , esta regla establece de manera muy compacta lo
siguiente:
46
𝑝(𝑏|𝑎 )𝑝(𝑎)
𝑝(𝑎|𝑏) = 𝑝(𝑏)
(3.1)
Por su propia naturaleza, el SLAM-EKF requiere disponer de un mapa en el cual las entidades que
lo componen sean fácilmente parametrizables. Esto es, los elementos que componen el mapa deben
poder ser descritos utilizando un conjunto de parámetros que encajen de forma sencilla en el vector
de estado del sistema.
47
A la vista de las hipótesis realizadas, cabe enumerar las siguientes desventajas de esta clase de
algoritmos:
1. La premisa de partida del algoritmo, que supone una distribución Gaussiana para el estado
del sistema, puede no corresponderse con la realidad. En el mejor de los casos, las
linealizaciones introducidas en los modelos harán que las estimaciones de los momentos de
esta distribución degeneren a lo largo del tiempo no correspondiéndose con sus auténticos
valores.
2. El punto anterior indica además un grave problema de consistencia del algoritmo, que hace
que el nivel de confianza de la estimación obtenida no se corresponda con el auténtico error
cometido. Esto origina que la exactitud de los resultados obtenidos por el filtro sean a
menudo impredecibles, observándose saltos bruscos en la estimación sin causa aparente
alguna.
3. El coste computacional crece al cuadrado con el número de objetos contenidos en el mapa.
Este hecho limita su aplicación en tiempo real a mapas formados por unos pocos cientos de
objetos.
4. No siempre es sencillo o inmediato extraer características del entorno asimilables a una clase
particular de objeto. En ocasiones ni siquiera es preciso extraer información que pueda
describirse empleando primitivas geométricas simples como puntos, segmentos, arcos de
circunferencia o planos (por ejemplo, en el interior de una mina).
5. Es preciso disponer de un método de asociación de datos robusto que permita emparejar las
observaciones realizadas con los elementos contenidos en el mapa. Una asociación de datos
errónea provocará casi con total seguridad la divergencia irrecuperable de la estimación del
filtro.
A pesar de los inconvenientes que surgen al emplear esta solución, se trata de la más extendida
en la literatura y presenta interesantes propiedades que resultan muy atractivas:
2. Estas soluciones cuentan con una larga tradición, lo cual hace que su estructura, ventajas e
inconvenientes sean bien conocidos. Históricamente se han planteado múltiples soluciones
que intentan paliar algunos de los problemas anteriormente mencionados.
3. Al mantenerse la matriz de covarianzas del sistema completa, es capaz de cerrar bucles
exitosamente.
El filtro de Kalman se implementa (Figura 26) en donde:
Los filtros de Kalman estan basados en sistemas dinámicos lineales discretizados en el dominio del
tiempo. Para ser modelados se basan en una cadena de Markov [18] construida en operaciones
lineales perturbadas por ruido Gaussiano. El estado del sistema es representado como un vector de
números reales. En cada paso discreto de tiempo una operación lineal es aplicada al estado para
generar el nuevo estado, con parte de ruido mezclado, y opcionalmente alguna información
proveniente de los controles del sistema si estos son conocidos. Al mismo tiempo, otra operación
lineal mezclada con más ruido genera la salida observada del estado verdadero (escondido).
Los filtros de Kalman pueden ser considerados como análogos al modelo de Markov escondido, con
la diferencia clave que la variable de estado escondida toma valores en un espacio continuo (en
oposición a un espacio de estado discreto como es en el modelo de Markov escondido). Además, y
en contraste con el modelo de ruido Gaussiano utilizado por los filtros de Kalman, los modelos de
Markov escondidos pueden representar distribuciones arbitrarias para el siguiente valor de las
variables de estado. Notemos que hay una fuerte dualidad entre las ecuaciones de los filtros de
Kalman y las de los modelos de Markov escondidos [19] [20].
49
Es conocido, en la teoría, que los filtros de Kalman son óptimos en casos en que
a) el modelo coincide perfectamente con el sistema real
b) el ruido de entrada es ruido blanco
c) la covarianza del ruido es conocido exactamente.
Muchos métodos para la estimación de la covarianza del ruido han sido propuestos durante las
últimas décadas. Después de que las covarianzas son identificadas, es útil evaluar la performance del
filtro, por ejemplo, si es posible mejorar la calidad estimada del estado o no. Se sabe que si los
filtros de Kalman trabajan de manera óptima, la secuencia de innovación (el error de predicción de
salida) es un ruido blanco.
Para la evaluación del rendimiento del filtro es necesario inspeccionar la propiedad de testigo de las
innovaciones. Varios métodos diferentes pueden ser usados para este fin [21].
Los supuestos de transiciones de estado lineal y mediciones lineales con ruido gaussiano
añadido rara vez se cumplen en la práctica. Por ejemplo, un robot que se desplaza con una velocidad
de rotación y de traslación constante se desplaza típicamente sobre una trayectoria circular, que no
puede describirse mediante transiciones de estado próximas lineales. Esta observación, junto con la
suposición de creencias unimodal, hace que los filtros de Kalman sencillos, como se ha discutido
hasta ahora, no sean aplicables a todos los problemas de la robótica, excepto los más triviales.
El filtro extendido de Kalman (EKF) supera uno de estos supuestos: el supuesto de linealidad.
Aquí la suposición es que la siguiente probabilidad de estado y las probabilidades de medición están
gobernadas por funciones no lineales g y h, respectivamente:
𝑧𝑡 = ℎ(𝑥𝑡 ) + 𝛿𝑡 (3.3)
está representado por 𝑢𝑡 y al covarianza Σ𝑡 . Así, el EKF hereda del filtro de Kalman la
representación de la creencia básica, pero difiere en que esta creencia es solamente aproximada, no
exacta como fue el caso en los filtros de Kalman.
3.8 SLAM
3.8.1 INTRODUCCIÓN
La “solución” del problema del SLAM ha sido uno de los sucesos más notables para la
comunidad de los últimos anos. SLAM ha sido formulado y resuelto como problema teórico en
distintas formas. SLAM ha sido, también, implementado en diversos campos de la robótica móvil,
como los robots de interiores, en exteriores, robots subacuáticos y robots voladores. A un nivel
teórico y conceptual. Sin embargo, aún quedan varios problemas en la aplicación práctica de
soluciones más generales de SLAM, y particularmente en la utilización y construcción de mapas
perceptualmente ricos como parte de un algoritmo de SLAM [10].
51
El nacimiento del problema del SLAM ocurre durante la Conferencia sobre Robótica y
Automática del IEEE, en la ciudad de San Francisco, en 1986. Durante estos años, los métodos
probabilísticos comenzaban a ser introducidos en los campos de la robótica y la inteligencia
artificial. Un grupo de asistentes mantuvieron una conversación sobre la aplicación de métodos de
estimación a los problemas de mapeado y localización. Como resultado de esta conversación, se
reconoció que el mapeado consistente probabilístico era un problema fundamental de la robótica que
merecía ser tratado.
Durante los siguientes años se describen en diferentes estudios las bases estadísticas para
describir las relaciones entre los objetos de referencia (conocidos como landmarks) y la
manipulación de la incertidumbre geométrica. Un elemento clave de estos trabajos fue mostrar que
debía existir una gran correlación entre las estimaciones de la localización de diferentes landmarks,
y que esta debía crecer con las sucesivas observaciones.
Esto requeriría que el estimador de posición almacenara un gran vector de estados (de orden
igual al número de landmarks que se conserven del mapa), con un coste computacional escalado al
doble del cuadrado de landmarks.
52
El descubrimiento más importante fue que el problema del mapeado y la localización, una
vez formulado como un único problema de estimación, era de naturaleza convergente. Aun mas, se
reconoció que las correlaciones entre los objetos de referencia, que otros científicos intentaban
minimizar o eliminar, eran la pieza clave del problema y que, de hecho, cuantas más ricas fueran
estas correlaciones, mejor sería la solución [10].
Los puntos de referencia son características del entorno que pueden ser fácilmente
distinguidas y re-observadas. Son utilizadas por el robot para poder saber dónde se encuentra. Hay
una serie de requisitos para considerar una característica como un buen landmark [11]:
- Debe ser posible observarla desde distintas posiciones y ángulos.
- Debe ser lo suficientemente única como para poder ser distinguida de otras características. Es
decir, si uno observa dos características y las re-observa en el futuro, debe ser posible identificar
cual es cual. Cuanto más cerca se encuentran unas de otras, más difícil es realizar esta tarea.
- Aquellas características que decidamos deben ser landmarks, deben ser suficientes como para que
el robot no circule durante largos periodos de tiempo sin observar ningún landmarks, ya que el robot
podría perderse.
- Si decidimos que algo debe ser un landmark, este debe permanecer estacionario. Una referencia en
movimiento provocaría datos y estimaciones erróneas.
La extracción de características puede conseguirse de diversas formas. Explicamos el método
RANSAC
3.8.5 RANSAC
Una vez realizada esta estimación (Figura 28), RANSAC comprueba cuantas lecturas de su
muestra aleatoria están cerca de esta aproximación. Si el número de coincidencias es superior a un
límite, o consenso, podemos estar seguros de haber observado una línea recta, y por tanto un
segmento de pared (o puerta o ventana).
En cada nueva observación, extraemos todas las referencias que encontremos y las
asociamos a la referencia más cercana que exista en nuestra base de datos (y que hayamos visto más
de N veces). El cálculo de las distancias puede hacerse por distancia Euclidea.
Una vez asociada, la pareja de landmarks se analiza usando un filtro (como un filtro de
Kalman) que nos dice si podemos considerar, dentro de un margen de incertidumbre, que ambas
referencias se corresponden. De ser así, se incrementa el número de veces que se ha observado esta
landmark en 1. En caso contrario, se incluye esta referencia como una nueva landmark en la base de
datos vista 1 vez.
Tan pronto el proceso de extracción de características y la asociación de datos finaliza, el
proceso de SLAM puede considerarse dividido en tres fases:
- Actualizamos el estado estimado del robot usando la información dada por la
odometría. Conociendo las acciones de control aplicadas al robot y la posición
(estimada) anterior, es fácil estimar la posición actual.
- Corregir la estimación tras la re-observación de los landmarks. Usando la estimación
de la posición es posible estimar dónde deberían estar ubicados. Usualmente,
encontraremos diferencias. A estas diferencias las llamaremos “innovación”.
Básicamente, la innovación es la diferencia entre la posición estimada del robot y la
posición real del robot, basada en lo que el robot es capaz de “ver”. En este paso, la
incertidumbre de cada landmark re-observado es actualizada también para reflejar los
cambios recientes.
- Finalmente, comenzaremos nuevamente el ciclo, añadiendo nuevos land- marks al
mapa del robot. Utilizaremos la información actualizada sobre posición e
incertidumbre obtenida en este paso.
Así pues, el proceso del SLAM puede esquematizarse de la siguiente manera (Figura 29):
56
3.8.7 MAPEADO
El objetivo del mapeado es representar la información obtenida del entorno de forma que sea
posible utilizarla para planificar la navegación y la resolución de tareas. Existen dos enfoques
diferentes: topológico y métrico.
Las representaciones topológicas únicamente consideran “lugares” y las conexiones entre
ellos. Estos mapas tienen forma de grafos, donde los nodos representan distintas posiciones
alcanzables por el robot y las aristas, la posibilidad de acceder a uno desde el otro. Es posible
almacenar las distancias entre posiciones como peso de las aristas que las unen. Las
representaciones geométricas consideran el espacio bidimensional en el que se producirá el
desplazamiento y ubica en éste los objetos. Las coordenadas de los objetos son elegidas con
precisión. Esta es la representación preferida al resolver el problema del SLAM.
Un ejemplo de representación geométrica es la generación de mapas por celdillas de
ocupación. El método se basa en discretizar el espacio, dividiéndolo en unidades de tamaño
predefinido, que se clasifican como ocupadas o vacías con un determinado nivel de confianza o
probabilidad.
57
3.9 NAVEGACIÓN
Una forma básica de este problema es la producción de un movimiento continuo que conecte
una posición inicial y una posición final, evitando cualquier tipo de colisiones con los obstáculos
que existan en el entorno (paredes, muebles, escaleras, etc.).
Cuando el número de dimensiones en las que puede desplazarse el robot es reducido, es
posible resolver este problema con algoritmos que discretica el espacio aplicando un patrón de
58
- Típicamente, es mucho más rápido comprobar solo segmentos cercanos entre marcas,
en contraposición a comprobar todas las posibles uniones.
- Distribuciones de muestreo no uniformes intentan ubicar marcas en areas que
mejoran la conectividad del camino.
59
Un algoritmo de búsqueda es aquel que está diseñado para localizar un elemento con ciertas
propiedades dentro de una estructura de datos; por ejemplo, ubicar la celdilla de destino en una
matriz de posiciones.
Aunque existe un inmenso número de algoritmos de búsqueda, nos centraremos en dos
estilos, de los que pueden derivarse todos los demás:
BFS es una estrategia de búsqueda en grafos donde la búsqueda está limitada esencialmente
a dos operaciones: visitar e inspeccionar un nodo del grafo y obtener acceso para visitar los nodos
vecinos del nodo siendo visitado. La búsqueda primero en anchura comienza en un nodo inicial, o
raíz, y desde ahí inspecciona todos sus vecinos. Después, para cada uno de esos vecinos,
inspecciona cada uno de sus correspondientes vecinos.
BFS es un algoritmo de búsqueda no informada que intenta expandir todos los nodos de un
grafo, buscando sistemáticamente cada una de las posibles soluciones. Es decir, que recorre todo el
grafo hasta dar con la solución, pero sin considerarla hasta que la encuentra.
Estos algoritmos pueden ser utilizados para resolver múltiples problemas en la teoría de grafos, pero
el más importante en el campo de la robótica es aquel que es capaz de encontrar siempre la distancia
más corta entre dos nodos dados. Es decir, BFS siempre devuelve el camino optimo, aunque no
puede garantizarse que encuentre la solución en un tiempo finito.
DFS es un algoritmo de búsqueda no informada que progresa al expandir el primer “hijo” del
nodo actual y repitiendo recursivamente esta operación hasta encontrar una solución o un nodo
60
“hoja”, es decir, un nodo sin “hijos”. En este caso, retrocederá, también recursivamente, hasta
encontrar el primer nodo anterior con “hijos” que expandir.
En el ámbito de la robótica, los algoritmos de búsqueda en profundidad, así como sus
variantes informadas, tienen multitud de aplicaciones como la búsqueda de nodos conectados (que
nos permite determinar si un determinado problema tiene solución) y, fundamentalmente,
encontrando el primer camino que une dos nodos.
4
ROS
________________________________________________________________________________________
Una vez instalado (Figura 31) correctamente se podrá acceder a su entorno con la instrucción
Gazebo posee una librería de modelos de robots diversos principalmente aquellos de más uso por la
comunidad de robotica como: Darpa, Drone, Turtlebot.
4.3.1 Sensado
Para la simulación en Gazebo se usara el robot Turtlebot, esta cuenta con un sensor Kinect
como se puede observar en la (Figura 33) una vez instalada podemos iniciar con la iniciación del
robot Turtlebot con la instrucción (Figura 34):
Figura 34: Comando para abrir el interfaz Gazebo con el robot Turtlebot.
Una vez iniciado este comando iniciara Gazebo en el cual se podrá ver al robot Turtlebot en
un entorno virtual, el cual también contara con más objetos, tales como uno puede ver en la (Figura
35).
Estos objetos se encuentra de forma aleatoria para poder ser utilizadas en las pruebas con el
robot Turtlebot como se ira mostrando en los demas puntos.
Si usamos la instrucción:
$rqt_graph, podremos ver como esta enlazados los nodos de ros con Gazebo (Figura 36)
65
Seguidamente para probar el sensor de proximidad se usara un código (Figura 37) el cual se
encargara de realizar las mediciones desde la ubicación del robot Turtlebot hasta el objeto más
cercano
Figura 37: Programa para determinar la distancia del robot Turtlebot hacia un objeto.
Se observa como las medidas obtenidas por el sensor varían a la vez que se modifica la
posición del objeto (Figura 38).
Figura 38: Imagen superior, muestra el valor de distancia entre el Robot y el objeto, en la imagen intermedio se puede ver como
varía la distancia y es proporcional a la distancia de cómo se aleja de este como en la imagen inferior.
El uso de sensores para poder determinar si existe un obstáculo en frente de éste es muy
utilizado para poder llegar a una autonomía en los robots, es tanto así que es una de las bases para
poder realizar un desarrollo avanzado de éste.
67
Es así que se probara con un código (Figura 39) para que pueda realizar desplazamiento y
evite obstáculos.
En este programa se comienza importando los mensajes tipo Lasers y Twist, se determina la
distancia en frente al robot, una vez obtenido el valor de la distancia se toma de referencia para
realizar una acción la cual es si no detecta un obstáculo en 30 segundos gira sobre su eje por un
tiempo de 5 segundos, o si este se encuentra frente a un obstáculo este deberá guiar obstáculo, la
distancia que se define como un obstáculo es cuando el valor de la distancia es menor a 0.8 metros.
4.3 RVIZ
Una vez instalado se realizara la prueba del sensor Kinect y probar en la ventana de
comandos (Figura 40):
En Rviz se visualiza tanto el robot que está siendo simulado y los valores de los sensores es
así que se visualiza en la imagen lo que está sensando (Figura 41 y 42).
Figura 42: Visualización de los valores obtenidos por la cámara Kinect del robot Turtlebot
Utilizando el nodo de teleoperación se observa que efectivamente se visualiza todos los objetos que
se ven en Gazebo (Figura 43).
Figura 43: En la imagen superior se observa los objetos visualizados por el sensor que con la ayuda de la ventana de teleoperacion se
va desplazando alrededor como la imagen inferior. .
70
4.4 GMAPPING
Se procede a correr los gmapping_demo.launch (), con el cual empezara a crear un mapa de
su entorno sin todavía mostrar su ubicación (Figura 44).
Con esto se realizara la construcción del mapa del entorno de GAZEBO usando el sensor
Kinect, a su vez se usara la librería de Teleoperacion con el cual se ira manejando manualmente para
poder posicionar y orientar el robot dentro de los objetos ubicados en gazebo.
En la siguiente imagen (Figura 45)se visualiza como está el entorno virtual en Gazebo.y en la (Figura
46) se observa la construcción del mapa alrededor de turtlebot.
5
DESARROLLO DE ALGORITMO PARA ROBOT
AUTÓNOMO
________________________________________________________________________________________
Con la finalidad de poder tener una placa embebida con la capacidad de trabajar sobre una estructura
móvil es que se realiza las pruebas en placas embebidas como raspberry pi y la upbaord, en la tabla
1 muestra una comparativa de 3 tarjetas diferentes.
TABLA 1
En la (Figura 47) se observa la placa Upboard a lo cual se conectó los dispositivos externos como
mouse, teclado pantalla, y una alimentación de 5 voltios de 4A.
73
Figura 47: Sistema embebido Upboard usado para realizar las pruebas.
Para su instalación se requirió de una memoria booteable, con el sistema operativo Ubuntu
14.04, una vez energizada la placa UpBoard comenzara con el arranque. Después de unos segundos,
el logotipo de UP debería aparecer en su pantalla (Figura 48). El sistema comenzará a arrancar
desde el instalador de Ubuntu en la unidad flash USB. Se presentará el gestor de arranque para
instalar Ubuntu en la placa UP, seleccione la opción "Instalar Ubuntu".
Una vez instalado se requerirá seguir todos los pasos que se toman a la hora de instalar
Ubuntu, una vez finalizado la instalación se procederá a actualizar el sistema para tener la última
actualización, abriendo a la ventana de terminal se ejecutara lo siguiente:
$ uname –r
Si la salida indica que está ejecutando un kernel con la cadena hacia arriba en el nombre, ya
está lista para comenzar como se muestra en la (Figura 49):
Una vez instalado el kernel correcto se procederá a añadir las librerías de Ros con el
siguiente código:
Agregar setup.bash al archivo .bashrc que garantiza que el marco ROS esté disponible en el
entorno activo.
La instalación del paquete ros-indigo-librealsense construirá una nueva imagen del núcleo
con los últimos parches RealSense aplicados. Una vez instalado, deberá reiniciar la UpBoard para
aprovechar ese kernel. Como antes, puede reiniciar a través de:
$ sudo reboot
Una vez que el sistema se haya reiniciado, puede la imagen del núcleo a través de uname –r
(Figura 50):
Para desarrollar el algoritmo nos basamos en el objetivo principal que es mover un robot
desde el principio hacia su posición objetivo, sin hacer colisión con el medio ambiente con la
finalidad de realizar una navegación autónoma.
75
La principal fuente de reconocimiento del entorno son los sensores es así que se necesita
alimentar la posición del robot y la odometría del robot, esta será enviada por los sensores como
encoder IMU, GPS, cámaras, una vez procesada la información esté tomará una acción que será
aplicada sobre los motores para darle su posición final (Figura 51).
de creencia sobre el error de predicción del modelo. El funcionamiento como podemos ver es muy
simple pero intuitivo y de lo que se trata es de predecir cuál será el estado de nuestro sistema en el
ciclo de predicción y posteriormente ajustar esa predicción en el ciclo de actualización. En resumen,
el procedimiento completo de estimación es el siguiente: el modelo es descrito en forma de espacio-
estado por su potencia descriptiva y para un conjunto inicial de parámetros dados, los errores de
predicción del modelo son generados por el filtro gracias a como lo hemos modelado y a la
información que nos proporcionan los sensores. Los errores en el modelo son utilizados para evaluar
iterativamente la función de creencia hasta maximizarla para obtener la estimación que maximiza la
creencia según las asunciones sobre los modelos de ruido, Algoritmo Filtro de Kalman.
El entorno para un robot es un sistema dinámico que posee estados tanto observables como
ocultos. El robot puede adquirir información del entorno por medio de los sensores aunque estas
suelen estar afectadas por ruido, y por lo general hay muchas características de nuestro entorno que
no pueden ser medidas de forma directa por parte de los sensores. Como consecuencia, el robot
mantiene una creencia que tiene relación con su entorno y los estados relacionados. El robot
también es capaz de influir sobre el entorno con el uso de actuadores. Hay que tener en cuenta que el
efecto de interactuar con el entorno por medio de los actuadores es algo impredecible.
En el desarrollo de este trabajo denotaremos los estados con su notación más habitual 𝑥, este vector
contendrá los estados específicos que necesitemos. El estado en el tiempo 𝑡 lo denotaremos como
𝑥𝑡 . Los estados más utilizados en el campo de la robótica móvil y por lo tanto en este trabajo son los
siguientes:
La pose del robot, que daría información sobre la localización y orientación de este relativo a
sistema de coordenadas global. Los robots móviles poseen seis grados de libertad, que se traducen
77
Un estado 𝑥𝑡 puede ser llamado óptimo si es el mejor predictor de las situaciones futuras que puedan
darse. Estos estados se estiman por medio de lo que se conoce como estimación estocástica.
Medidas de sensores: La percepción es el proceso por medio del que el robot obtiene información
de los sensores acerca de los estados del entorno.
La medida de los sensores en el tiempo 𝒕 será denotado por 𝒛𝒕 .
Acciones de control: Las acciones de control cambian los estados del entorno Algunos ejemplos
podrían ser el control del movimiento de un robot (envío de comandos a las ruedas para alcanzar
una cierta consigna).
La acción de control será denotada como 𝒖𝒕 .
La diferencia entre medida y acción de control es crucial. Como ya hemos dicho la percepción
proporciona información acerca de las variables de estado del entorno y por lo tanto intenta
aumentar el conocimiento del entorno. El movimiento, por otra parte lo que produce es una pérdida
de conocimiento sobre los objetos que tenemos a nuestro alrededor debido al ruido asociado a los
actuadores aunque a veces una acción de control apropiada puede llegar a lograr un movimiento
bastante preciso. Esto no significa que se tengan que dar por separado la medida y la acción de
control, ya hemos dicho que pueden darse simultáneamente.
La evolución de los estados y las medidas se considera que siguen el modelo probabilista de
inferencia Bayesiana
El robot ejecuta la acción de control 𝑢1 primero, y luego realiza las medidas Sin embargo, si el
estado 𝑥 es completo será suficiente la información que contiene como para saber lo que ha
ocurrido con estados previos. En particular, 𝑥𝑡−1 sería suficiente información de todos los estados
previos de control y medida hasta ese instante de tiempo, serían 𝑢1:𝑡−1 𝑦 𝑧1:𝑡−1 , por lo que no
necesitaríamos almacenar información más allá del estado anterior. Por lo tanto la ecuación (5.1)
quedaría de la siguiente forma:
79
Esta propiedad es lo que conocemos como independencia condicional, que es una de las reglas de
Markov. Por otro lado algo muy similar ocurre con las medidas que generamos a partir de nuestro
modelo. De nuevo, si 𝑥𝑡 es completo tenemos:
Lo que significa que el estado 𝑥𝑡 es suficiente para predecir las medidas 𝑧𝑡 (estando estas afectadas
por ruido).
Otro concepto muy importante en la robótica probabilística es la creencia (belief). La creencia nos
da una idea del conocimiento interno del robot acerca de los estados del entorno, es decir, no aporta
conocimiento sobre como de precisa es la estimación de un estado.
También podríamos calcular una distribución previa a incorporar la medida, y justo después de
ejecutar el control 𝑢𝑡 se denotara de la siguiente forma:
Esta distribución de probabilidad será lo que conozcamos como predicción. Y por lo tanto calcular
𝒃𝒆𝒍(𝒙𝒕 ) a partir de ̅̅̅̅̅
𝒃𝒆𝒍(𝒙𝒕 ) es lo que conocemos como corrección o actualización.
80
El algoritmo más extendido para calcular la certidumbre viene dado por lo que se conocen como
filtros Bayesianos. Estos filtros calculan una distribución de certidumbre 𝑏𝑒𝑙( ) a partir de las
medidas y las acciones de control.
Podemos apreciar que este algoritmo es iterativo, esto quiere decir que la certidumbre 𝒃𝒆𝒍(𝒙𝒕 ) en
un tiempo 𝒕 se calcula a partir de la certidumbre anterior 𝒃𝒆𝒍(𝒙𝒕−𝟏 ) en el tiempo 𝒕 − 𝟏. Otro
detalle muy importante es que hay una dependencia temporal de forma secuencial para 𝒙. Los datos
de partida para la realización del cálculo son la certidumbre en el tiempo 𝒕 − 𝟏, el estado de la
acción de control más reciente 𝒖𝒕 y la medida más reciente 𝒛𝒕 . Como resultado el algoritmo nos
devuelve la confianza 𝒃𝒆𝒍(𝒙𝒕 ) en el tiempo 𝒕.
En la línea 2, lo primero que haremos será procesar la certidumbre en función del estado de control
𝒖𝒕 . Calculamos la certidumbre del estado 𝒙𝒕 basándonos en la certidumbre del estado 𝒙𝒕−𝟏 y de la
acción de control actual 𝒖𝒕 . En concreto, como podemos ver la certidumbre ̅̅̅̅̅
𝒃𝒆𝒍(𝒙𝒕 ) que el robot
asigna al estado 𝒙𝒕 es obtenida por medio de la integral del producto de ambas distribuciones. Todo
este proceso es lo que conocemos como ciclo de predicción.
En la línea 3 ocurre la etapa de corrección o actualización. En esta línea, vemos como multiplicamos
la certidumbre ̅̅̅̅̅
𝒃𝒆𝒍(𝒙𝒕 ) por la probabilidad de las medidas 𝒛𝒕 que hemos tomado. Una vez hecho
esto y normalizado el resultado obtendremos una certidumbre final 𝒃𝒆𝒍(𝒙𝒕 ) , que devolvemos en la
línea 5.
81
Se extrae del ANEXO A la ecuación (A.14) conocida como la Distribución Normal, asumimos que
𝒙 es un valor escalar, a menudo 𝒙 puede ser un vector multi-dimensional. La Distribución Normal
sobre vectores son llamados multivariable caracterizados por la siguiente forma:
1
(𝑥−𝜇)𝑇 Σ−1 (𝑥−𝑢)}
𝑝(𝑥) = det(2πΣ)−1/2 𝑒 {−2 (5.6)
1.- La siguiente probabilidad 𝒑(𝒙𝒕 | 𝒖𝒕 , 𝒙𝒕−𝟏 ) para ser una función lineal necesita añadirle el ruido
Gaussiano, expresado por la siguiente ecuación.
𝑥𝑡 = 𝐴𝑡 𝑥𝑡−1 + 𝐵𝑡 𝑢𝑡 + 𝜀𝑡 (5.7)
𝑥1,𝑡
𝑥2,𝑡
𝑥𝑡 = : (5.8)
𝑥𝑛,𝑡
𝑢1,𝑡
𝑢2,𝑡
𝑢𝑡 = : (5.9)
𝑢𝑛,𝑡
La ecuación (5.7) define que la probabilidad de transición de estado 𝒑(𝒙𝒕 | 𝒖𝒕 , 𝒙𝒕−𝟏 ). Esta
probabilidad de obtiene al utilizar la ecuación (5.7) en la definición de multivariable (5.6), donde la
media de estado posterior viene dada por 𝑨𝒕 𝒙𝒕−𝟏 + 𝑩𝒕 𝒖𝒕 y la covarianza por 𝑹𝒕 .
1
(𝑥𝑡 −𝐴𝑡 𝑥𝑡−1 −𝐵𝑡 𝑢𝑡 )𝑇 𝑅𝑡 −1 (𝑥𝑡 −𝐴𝑡 𝑥𝑡−1 −𝐵𝑡 𝑢𝑡 )}
𝑝(𝑥𝑡 | 𝑢𝑡 , 𝑥𝑡−1 ) = det(2π𝑅𝑡 )−1/2 𝑒 {−2 (5.10)
2.- La probabilidad de las medidas 𝒑(𝒛𝒕 | 𝒙𝒕 ) también debe tener argumentos lineales, con ruido
𝑧𝑡 = 𝐶𝑡 𝑥𝑡 + 𝛿𝑡 (5.11)
Donde 𝑪𝒕 es una matriz 𝒌 ∗ 𝒏 donde 𝒌 es la dimensión del vector 𝒛𝒕 , el vector 𝜹𝒕 describe la medida
del ruido, la distribución de 𝜹𝒕 es una multivariable Gaussiana con una media de cero y covarianza
𝑸𝒕 . La medida de la probabilidad viene dada por:
1
(𝑧𝑡 −𝐶𝑡 𝑥𝑡 )𝑇 𝑄𝑡 −1 (𝑧𝑡 −𝐶𝑡 𝑥𝑡 )}
𝑝(𝑧𝑡 | 𝑥𝑡 ) = det(2π𝑄𝑡 )−1/2 𝑒 {−2 (5.12)
3.- Finalmente la predicción inicial 𝑏𝑒𝑙(𝑥0 ) para ser una distribución normal vamos a denotar la
predicción por 𝝁𝟎 y la covarianza por 𝚺𝟎 :
1
(𝑥0 − 𝜇0 )𝑇 Σ0 −1 (𝑥0 −𝜇0 )}
𝑏𝑒𝑙(𝑥0 ) = 𝑝(𝑥0 ) = det(2πΣ0 )−1/2 𝑒 {−2 (5.13)
Estas tres suposiciones son suficientes para garantizar que el prediccion posterior 𝒃𝒆𝒍(𝒙𝒕 ) sea
siempre gaussiano, para cualquier punto en el tiempo t.
83
Una vez ya tenemos claros todos los principios sobre los que se sostiene el filtro de Kalman clásico,
podemos pasar a su implementación en un algoritmo. Podemos ver el planteamiento en el algoritmo
de la (Figura 53). Las entradas al algoritmo son la certidumbre en un tiempo 𝒕 − 𝟏 , representada
por su media 𝒖𝒕−𝟏 y su covarianza 𝚺𝒕−𝟏 . Para la actualización de estos parámetros necesitaremos el
vector de control 𝒖𝒕 y las medidas tomadas en el instante actual 𝒛𝒕 . Podemos ver que la salida del
algoritmo es la nueva media 𝒖𝒕 y la nueva covarianza 𝚺𝒕 de la certidumbre en el instante 𝒕.
84
En la (Figura 54) muestra un ejemplo simple sobre el funcionamiento del filtro en un espacio
unidimensional. Suponemos que el robot realizará movimientos hacia la derecha en el eje x. La
probabilidad a priori de la localización del robot se muestra en la subfigura 1, donde podemos
apreciar que la hemos caracterizado siguiendo una distribución Gaussiana. El robot realizará las
medidas del entorno por medio de sus sensores (ultrasonidos, etc.) y estos devolverán una medida
representada mediante otra Gaussiana cuya media será el valor de medida como podemos ver en la
subfigura 2. La Gaussiana de la medida tiene el valor de pico centrado en el devuelto por los
sensores, en cuanto a la varianza, esta corresponde a la incertidumbre existente en dicha medida.
Combinando la probabilidad a priori con la medida obtenemos la probabilidad a posteriori que
podemos ver en la subfigura 3. Podemos ver que la certidumbre resultante se encuentra entre las dos
medias originales, y la incertidumbre que tiene la nueva Gaussiana es menor que el de las dos
Gaussianas anteriores. El hecho de que la incertidumbre de la Gaussiana resultante sea menor que el
de las anteriores puede parecer poco intuitivo. Este hecho es característico de la integración de la
información en el filtro de Kalman donde en general cuanta más información tenemos disponible
menos incertidumbre tendremos. Siguiendo con el ejemplo, ahora suponemos que el robot realiza un
movimiento hacia la derecha. En la subfigura 4 podemos ver como la incertidumbre ha aumentado
además de que se ha desplazado en la misma medida que el robot lo ha hecho en el eje hacia la
derecha. En la subfigura 5 el robot vuelve a realizar una medida, que combinada con la probabilidad
85
a priori da como resultado una nueva certidumbre que se muestra en la subfigura 6. El filtro de
Kalman alterna entre el ciclo de actualización donde integra las medidas tomadas por los sensores
con el ciclo de predicción que modifica la certidumbre conforme a las acciones que ha realizado el
robot. El ciclo de actualización reduce la incertidumbre y el ciclo de predicción por lo general la
aumenta.
La suposición de que los sistemas tienen transiciones lineales entre estados y que las medidas
también son lineales afectadas con ruido Gaussiano aditivo raramente se cumple en la práctica. Un
claro ejemplo de la afirmación anterior sería un robot quese mueve con velocidad lineal y angular
constante describiendo una trayectoria circular, esta transición de estados no podría ser descrita por
ningún tipo de función lineal. Esta suposición, junto con la suposición de que la certidumbre sigue
una distribución unimodal, hace que el KF sea una aproximación demasiado simple, por lo que sería
aplicable solamente a problemas triviales de robótica siendo inaplicable en el resto de casos.
El EKF supera al KF en cuanto a uno de sus supuestos básicos, la linealidad. En el EKF la transición
entre estados y las medidas están descritas por funciones no lineales 𝒈 y 𝒉 respectivamente.
Este modelo estrictamente generaliza el modelo lineal Gaussiano en el que se basa el KF, este
modelo lo podemos ver en las ecuaciones (5.7) y (5.11). La función 𝒈 remplaza las matrices 𝑨𝒕 y 𝑩𝒕
de la ecuación (5.7) y 𝒉 reemplaza la matriz 𝑪𝒕 de la ecuación (5.11).
La evolución de los estados al usar matrices 𝒈 y 𝒉 seleccionadas arbitrariamente no presentan una
certidumbre distribuida siguiendo una Normal como si ocurría en el KF. De hecho, la realización de
la actualización de la certidumbre es normalmente imposible de desarrollar para funciones no
lineales 𝒈 y 𝒉, ya que los filtros Bayesianos no tienen un método para efectuar este cálculo.
El EKF calcula una aproximación a la certidumbre real por medio de una distribución Gaussiana. En
particular, la certidumbre 𝒃𝒆𝒍(𝒙𝒕 ) en un tiempo 𝒕 es representada por una media 𝒖𝒕 y una covarianza
86
𝚺𝒕 . Así el EKF hereda del KF la forma de representar la certidumbre, pero se diferencia en que esta
será una mera aproximación y no es calculado como en el KF.
Como ya hemos dicho uno de los puntos básicos del EKF es la representación de la certidumbre
mediante una aproximación de esta. Por lo tanto la idea principal es la linealización. Si suponemos
que tenemos una función no lineal 𝒈 una función Gaussiana proyectada a través de esta función se
convertirá en no Gaussiana. Esto es porque existen no linealidades en 𝒈 que perturban la
certidumbre de tal manera que hacen que no puede ajustarse a una Gaussiana. Por lo tanto con la
linealización lo que buscamos es aproximar la función 𝒈 con una función lineal que sea tangente a
la función 𝒈 en la media de la Gaussiana. Proyectando la Gaussiana por medio de esta aproximación
lineal, la distribución de probabilidad a posteriori si será una Gaussiana. De hecho, una vez que 𝒈 ha
sido linealizada el método para la propagación de la certidumbre es igual que para el KF. La misma
explicación se aplica a la multiplicación de Gaussianas cuando se utiliza la función 𝒉. Para este caso
el EKF vuelve a aproximar la función 𝒉 por medio de una función lineal que es tangente a 𝒉,
manteniendo así la naturaleza Gaussiana de la probabilidad a posteriori.
A la hora de linealizar funciones no lineales pueden utilizarse una gran variedad de técnicas. En
concreto el EKF utiliza un método llamado expansión de Taylor. En el EKF se suelen trabajar con
expansiones de primer y segundo orden. La expansión de Taylor constituye una aproximación lineal
a una función 𝒈 por medio del valor de sus derivadas y su pendiente. La pendiente viene dada por la
derivada parcial siguiente:
𝜕𝑔(𝑢𝑡 ,𝑥𝑡−1 )
𝑔′(𝑢𝑡 , 𝑥𝑡−1 ) ≔ (5.16)
𝜕𝑥𝑡−1
Está claro que el valor de g y su pendiente dependerá de los argumentos de 𝒈 (vector de estados y de
control). Una elección lógica para seleccionar los argumentos es elegir el estado que se considera
más probable en el instante en el que se realiza la linealización. Para las distribuciones Gaussianas,
el estado más probable es la media del posterior estado de control 𝝁𝒕−𝟏. En otras palabras, 𝒈 es
aproximada por su valor en el instante en que se da 𝝁𝒕−𝟏 y 𝒖𝒕 . La linealización por tanto se
conseguiría de la siguiente forma:
87
La matriz 𝑮𝒕 es una matriz de dimensiones nxn, donde n es la dimensión del vector de estados. Esta
matriz se conoce con el nombre de Jacobiano. El valor del Jacobiano depende de 𝒖𝒕 y 𝝁𝒕−𝟏, por lo
tanto se diferencia para distintos instantes de tiempo. La misma linealización es implementada para
la función de medida 𝒉. En esta expresión la serie de Taylor se desarrolla alrededor de 𝝁
̅ 𝒕 , que es el
estado que se considera más probable para linealizar la función de la siguiente manera:
𝜕ℎ(𝑥𝑡 )
Sabiendo que ℎ′(𝑥𝑡) = podemos escribir la expresión en forma de Gaussiana:
𝜕𝑥𝑡
1 𝑇
̅ 𝑡 )) Q𝑡 −1 (𝑧𝑡 −ℎ(𝜇
)−𝐻𝑡 (𝑥𝑡 −𝜇 ̅ 𝑡 )− 𝐻𝑡 (𝑥𝑡 −𝜇
̅ 𝑡 ))}
𝑝(𝑧𝑡 | 𝑥𝑡 ) = det(2πQ𝑡 )−1/2 𝑒 {−2(𝑧𝑡−ℎ(𝜇̅𝑡
En el algoritmo de la (Figura 55) podemos ver las ecuaciones que usa el EKF para funcionar. Como
podemos apreciar este algoritmo es muy parecido al algoritmo de la (Figura 53) donde se aplicaba el
KF. Las diferencias más importantes las podemos apreciar en las líneas 1 y 4 en ambos algoritmos.
88
Los valores recibidos por la Odometría se describen por seis variables, las coordenadas
cartesianas en 3 dimensiones (x y z) y los tres ángulos de Euler (roll, pitch y yaw) los cuales se
pueden ver en la siguiente (Figura 56):
Fuente de sensor: Para poder mapear el entorno del robot es que se necesita de los datos de
escaneo ya sea de un sensor laser, un sensor Kinect o cámara con sensor de profundidad, estos datos
89
se combinan para construir un mapa local del robot, en la siguiente (Figura 57) se puede ver los
valores obtenidos por el sensor.
Configuración de Transformación: Considerando un robot que tiene una base móvil éste tiene
dos cuadros de coordenadas uno correspondiente al punto central de la base del robot y otro para el
punto central del láser que está montado en la parte superior de la base, si quisiéramos utilizar esta
información para ayudar a la base móvil a evitar obstáculos necesitamos una forma de transformar el
escaneo laser al punto central de la base del robot como el que se muestra en la siguiente (Figura 58)
Figura 59: Valores a ser enviados a los motores del robot Turtlebot
Cuando se recibe un objetivo, se vincula con el mapa generado y envía al controlador base
para mover el robot con la finalidad de lograr su objetivo.
Planificador Global: Para poder definir la trayectoria óptima desde la posición actual hasta la
posición de la meta con respecto al mapa del robot, usando un algoritmo para definir la ruta como el
algoritmo A*:
Mapa de Costos: Encargado de realizar un mapeo del entorno del robot con el cual el
planificador Global tomara sus decisiones (Figura 60).
91
Gmapping: Es una implementación de un algoritmo llamado SLAM que toma los datos del
escáner láser y la odometría para construir un mapa de cuadrícula de ocupación 2D.
Se diseñó un entorno cerrado (Figura 61) el cual servirá para realizar las pruebas del robot, este
diseño se creó utilizando el bloque Jersey Barrier el cual se puede sobreponer sobre otros bloques y
quedar de forma sólida.
Una vez diseñado éste, se procederá a realizar el mapeo del entorno para la creación del
mapa:
Para esto es que se ejecutaron los códigos de SLAM para que empieze con el código de
mapeo y con el código de teleoperación para ir controlando de manera remota al robot y este puedas
ir almacenando los datos de odometría y del sensor Kinect (Figura 62).
92
Teclas para
manejar al
robot
Visualización de entorno
en 3d a través de la
cámara del robot
Ubicación del
robot
Una vez creado el entorno se almacena y se vuelve a ejecutar el código para SLAM y darle
metas para que éste pueda llegar a su destino, en la imagen (Figura 64) se puede apreciar el color
verde que indica todas las posibles posiciones que podría tomar el robot dependiendo de la ruta que
se le dé en ambas figuras se observa que el robot está en la posición original.
Ubicación del
Ubicación del robot en robot en el
el programa RVIZ simulador
Se va a la opción para indicarle la meta (Figura 65), se puede ver que aparece en la figura una flecha
de color verde este indica la posición que desea que tome el robot (meta) una vez decidido la meta el
robot decidirá el mejor camino para poder llegar a la meta:
Posición inicial
del robot
En la figura 66 y 67 se puede ver que en la ventana de la derecha aparece una línea roja que vendría a
ser el camino decidido por el robot para poder llegar a su meta
Figura 67: Se ve como el robot en el simulador se mueve de forma autónoma sin tocar los obstáculos
Se diseñó un segundo entorno cerrado (Figura 68) para realizar las pruebas de entorno.
Ubicación
del robot en
la simulación
Una vez realizado la construcción del mapa se volvió a centrar el robot según su posición original de
la simulación.
La línea amarilla
muestra el
obscatulo mostrado Ubicación
por el sensor del robot en
la simulación
Es así que se definió la meta que seguiría el robot, para esto al haber un obstáculo en frente de sí
mismo es que tuvo que tomar un camino que le ayude evitar el obstáculo tal cual como muestra
(Figura 70).
Posición inicial del robot La línea roja indica el camino Posicion final de robot
a recorrer según el algoritmo
Posición final
(meta) y
orientación
que se le
indica al Figura 70: Evasión de obstáculo.
robot a seguir
97
Para poder usar los sensores se apoyó de una placa (Arduino) para la integración de un sensor
ultrasonido y este puedo enviar los datos vía transmisión serial y sustituir los valores del sensor
simulado (Figura 71).
La figura muestra el “arduino Mega” conectado a la “Upboard” para este ejemplo se conectó un
Sensor ultrasonido, es así que se conectó un sensor ultrasonido HC-SR04 según se muestra (Figura
72):
Este mismo se conectó hacia la Upboard por su puerto Serial y se le cargo un programa para lectura
de datos y los envié hacia la Upboard (Anexo D)(Figura 73).
Alimentación 5v
Sensor
ultrasonido
Una vez realizado la conexión y el energizado se empezó realizar la lectura de los valores en la
(Figura 74) se puede ver que la distancia disminuye a medida que se acerca el sensor hacia la pared.
Una vez se pueda ver los valores del sensor se integrara dentro de Gazebo para sustituir con el
sensor simulado, para se ejecuta un código para que el robot evada obstáculos (Figura 75) usando el
sensor ultrasonido (Figura 76).
Para la construcción de robot se usa la plataforma de Raspberry pi debido a que la versión 3 posee
comunicación wifi, mediante esto se comunicaría a la PC para él envió de datos y construcción del
mapa (Figura 77).
El circuito para la construccion del robot autonomo fue la siguiente (Figura 78).
Connexion USB
Para la consutruccion del robot se utilizo un sensor laser, raspberry 3 modelo B , Board con
coneccion a Arduino , salida TTL y motores como se muestra en la (Figura 79).
Una vez realizado el la configuracion y se haya cargado ros en la raspberry se procedio con las
pruebas para el desarrollo del mapeo.
Primero se realizo la lectua del sensor laser para realizar el escaneo en 360° (figura 80) se observa
que los puntos rojos muestra a los objetos que se encuentran alrededor de este.
Seguidamente se verifico la comunicacion entre el robot y la PC a travez de la red wifi, es asi que se
procedio a la generacion de mapas mediante el SLAM como se muestra (Figura 81).
102
Ubicación del
Ubicación del Robot
Robot
6
CONCLUSIONES Y TRABAJO A FUTURO
________________________________________________________________________________________
6.1 CONCLUSIONES
Se desarrolló un algoritmo que con ayuda de los sensores y la odometría pueda realizar la
construcción de mapas del entorno basado en SLAM.
Se probó el algoritmo en el simulador de entornos Gazebo que corre en el entorno de ROS.
Se implementó la lógica en una placa embebida para simular los sensores que permitan obtener
valores de localización, objetos cercanos y cámara para adquisición de imágenes este ayudara a
crear un mapa virtual y reconocer objetos específicos para aplicaciones de búsqueda y
reconocimiento.
Los dos diseños de entornos realizados (Fig. 61, 62 y 69) muestran que la imagen creada difiere
a la imagen original creada en el simulador esto debido a las constantes lecturas que tienen un
grado de error producto de los sensores.
Debido a las prestaciones se eligió la plataforma Upboard para la simulación mientras que para
la construcción la raspberry pi.
El mapeo del lugar obtenido por el robot fueron similares a las obtenidas por simulación.
6.2 ALCANCES
El uso de sistemas embebidos nos ayuda a poder mejorar, desarrollar y armar este tipo de robot
de una manera más robusta sin la necesidad de usar Laptop.
104
Se puede utilizar las nuevas cámaras de desarrollo lanzadas por Intel para poder aprovechar el
USB 3.0 que nos da la Upboard.
Se puede usar las versiones mejoradas de ROS para trabajar con sus sistemas operativos más
actualizados.
6.3 RECOMENDACIONES
Bibliografia
[1] E. Lupu, “Aspects on path planning for mobile robots”, Reporte interno de la Technical
University of Cluj-Napoca. (2001).
[3]Ruiz, “Desarrollo de una plataforma de control para el robot móvil Roomba utilizando ROS e
integración de un se sensor Kinect”, Sartenejas (2012).
[4] Amidi O. “Integrated Mobile Robot Control. Robotics Institute, Carnegie Mellon University,
Pittsburgh, PA, Tech. Rep” (1990).
[5] James L,”World Modeling and Position Estimation for a Mobile Robot Using Ultrasonic
Ranging”, Grenoble (1982).
[9]Jorg Stuckler and Sven Behnke. “Robust real-time registration of rgbd images using multi-
resolution surfel representations”Ettenheim (2014).
[10]Hugh Durran-Whyte and Tim Bailey. “Simultaneous localisation and mapping (SLAM): Part I
the essential algorithms”, Robotics and Automation Magazine (2006).
[11] Soren Riisgaard and Morten Rufus. “Slam for dummies a tutorial approach to simultaneous
localization and mapping” Cognitive Robotics Spring (2005).
[14] Jungong Han “Enhanced Computer Vision With Microsoft Kinect Sensor: A Review” IEEE
Transactions on Cybernetics, Volume: 43 (2013).
[15] Frost & Sullivan “LiDAR: Driving the future of autonomous Navigation”, Lidar Technology
(2016).
[16] Adam Milstein “Occupancy Grid Maps for Localization and Mapping”, Motion Planning
(2008).
[17] R. Craig, “Implementacion of the Pure Pursuit Path Tracking Algorithm”, Pittsburgh
Pennsylvania (1992).
106
[18] J Laurie, Snell Charles, M Grinstead. “Introduction to Probability: Second Revised Edition.
American Mathematical Societ”, Providence (1997).
[19] Sam Roweis and Zoubin Ghahramani. “A unifying review of linear gaussian model”, Neural
Computation (1999).
[20] James Douglas Hamilton. “Time Series Analysis”, Princeton University Press (1994).
[21] Peter Matisko and Vladimir Havlena. “Optimality tests and adaptive Kalman filter”, IFAC
(2012)
[22] Rudolph E. Kalman. “A new approach to linear filtering and prediction problems” Transaction
of the ASME|Journal of Basic Engineering (1960).
[23] Peter S. Maybeck. “Stochastic models, estimation, and control”, UNC Computer Science
(1979).
[24] O. L. R. Jacobs. “Introduction to Control Theory”, Second ed. Wellington Square, Oxford OX1
2JD, United Kingdom (1993).
[25] Robert Grover Brown and Patrick Y. C. Hwang. “Introduction to Random Signals and Applied
Kalman Filtering: with MATLAB Exercises and Solutions”, Wiley & Sons, New Jersey (1996).
[26] Joaquin Viñals Pons “Localización y generación de mapas de un robot por medio de una
kinetc”, Universitat Politecnica de Valencia (2012).
A
ANEXO A: CONCEPTOS BASICOS PARA COMPRENDER
EL FILTRO DE KALMAN
La mayoría de nosotros tenemos alguna noción o idea de lo que significa un evento aleatorio, o la
probabilidad de que un evento pueda ocurrir en un espacio experimental. Un experimento aleatorio
se define como un experimento cuyo resultado es incierto, pero que se puede repetir (como podría
ser el lanzamiento de un dado y que salga un número concreto). El conjunto de todos los posibles
resultados de un experimento aleatorio se denomina espacio muestral, y se denomina a menudo con
la siguiente notación Ω. Formalmente, el resultado para la probabilidad de un evento discreto (por
ejemplo el lanzamiento de una moneda) a favor está definida de la siguiente manera (A.1)
En el caso de eventos dependientes la probabilidad de que ocurra A o B está dada por (A.2). Esta
expresión también se conoce como la unión de A y B.
Finalmente, si A y B son dos sucesos, la probabilidad de que ocurra B dado A (es decir que A ocurre
antes que B) se denota por p(B | A) y es llamada probabilidad condicional y viene dada por (A.4).
𝑝(𝐴 ∩𝐵)
𝑝(𝐴|𝐵) = (A.4)
𝑝(𝐵)
A.2.VARIABLES ALEATORIAS
Una variable aleatoria es una función del espacio muestral W en el conjunto de los números reales.
Las variables aleatorias se denominan generalmente X, Y, Z o con otra letra mayúscula escogida del
final del alfabeto. Por ejemplo,
𝑋: Ω ⟶ 𝑅
Define la variable aleatoria X como una función del espacio muestral en el conjunto de los números
reales.
Las variables aleatorias se clasifican de acuerdo a su recorrido. Si el número de valores que puede
tomar es finito o infinito numerable (tiene infinitos elementos que pueden ser numerados, como el
conjunto de números enteros), X se denomina variable aleatoria discreta. Si el recorrido de X es en
un conjunto de valores (por ejemplo, los valores de un intervalo de números reales), se denomina
variable aleatoria continua. En el caso de variables aleatorias continuas, la probabilidad de cualquier
evento simple discreto A es cero, esto es, P(A)=0. Una función común que representa la
probabilidad de una variable aleatoria, es definida como la función de distribución acumulativa. Esta
función (ecuación (A.5)) representa la probabilidad acumulativa de las variables aleatorias continuas
X para todos (los no numerables) eventos hasta x.
𝑑
𝑓𝑥 (𝑥) = 𝐹 (𝑥) (A.6)
𝑑𝑥 𝑥
+∞
∫−∞ 𝑓𝑥 (𝑥) 𝑑𝑥 = 1 (A.8)
A.3.MEDIA Y VARIANZA
La media para un espacio muestral N (se define como espacio muestral todos los posibles resultados
de un experimento estadístico, como comentamos en la sección anterior) de una variable aleatoria X,
el promedio, la media muestral o esperanza está dada por:
𝑋 +𝑋 +⋯+𝑋𝑁
𝑋̅ = 1 2𝑁 (A.10)
La media de una variable aleatoria X, se puede expresar como 𝜇𝑋 o simplemente 𝜇 cuando sabemos
a qué variable aleatoria está referido. También es común referirse a la media como el valor esperado
o esperanza, en cuyo caso se expresa como E(X).
Si X es una variable aleatoria discreta con distribución de probabilidad f(x) la media o valor
esperado de X vendría dado de la siguiente forma:
En el caso de que X sea una variable aleatoria continua tendríamos la siguiente expresión:
+∞
𝜇𝑋 = 𝐸(𝑋) = ∫−∞ 𝑓𝑥 (𝑥) 𝑑𝑥 (A.12)
La varianza en una propiedad estadística muy usada en las señales aleatorias, ya que si se supiera la
varianza de una señal que de otro modo fuera supuesta para ser constante alrededor de un valor, la
magnitud de la varianza daría una idea de cuánto nos estamos desviando con respecto a la media. En
el filtro de Kalman usaremos la varianza con el fin de saber cuánto ruido hay en nuestra medida o
como de deslocalizados nos encontramos en el mapa, este asunto lo trataremos en secciones
posteriores cuando hablemos de la propagación del error.
1 2 /2𝜎 2
𝑓(𝑥) = 𝜎2 𝑒 −(𝑥−𝜇) , −∞ < 𝑥 < ∞ (A.14)
√2𝜋
La notación típica para representar los valores de densidad de una variable aleatoria X con media 𝜇
y desviación típica 𝜎 es 𝑁(𝑥; 𝜇, 𝜎).
Podemos ver que en las ecuaciones (A.2) y (A.4), que la probabilidad ha sido definida para variables
aleatorias continuas. Dos variables aleatorias continúas X e Y son independientes estadísticamente
si la probabilidad conjunta 𝑓𝑥𝑦 (𝑥, 𝑦) es igual a la suma de sus probabilidades marginales, es decir,
que se consideran independientes sí que cumple lo siguiente:
𝑓𝑥𝑦 (𝑥, 𝑦) = 𝑓𝑥 (𝑥)𝑓𝑦 (𝑦) → 𝑝(𝑋 ∩ 𝑌) (A.15)
Un ejemplo algo más claro para ilustrar la independencia sería suponer que lanzamos una moneda
dos veces y que dicha moneda no está trucada. Sea A el suceso de que en el primer lanzamiento
salga cara y B el suceso de que en el segundo lanzamiento salga cara. Supongamos que A ocurre
¿Cambia eso la probabilidad de que B ocurra? La respuesta lógica que todos daríamos es que no y
efectivamente así es. Sabemos que el resultado del lanzamiento de la primera moneda no influye en
el resultado del segundo lanzamiento. En el caso de que dos eventos se condicionen entre sí en el
cálculo de la probabilidad (sacar bolas de colores de una bolsa) podríamos expresarla de forma
matemática. Esta idea se puede expresar matemáticamente usando el concepto de probabilidad
condicional que veíamos al principio de esta sección en la ecuación (A.4) donde expresaríamos la
probabilidad de que un evento ocurra cuando previamente ha ocurrido otro.
Otro concepto muy relevante a la hora de hablar de probabilidad condicional es el teorema de
Bayes. Para ilustrar la motivación de esta expresión nos remitiremos a un ejemplo. Imaginemos que
calculamos la probabilidad de que el resultado de un test de VIH realizado sobre un individuo
escogido aleatoriamente sea positivo. Para el individuo, es mucho más importante saber si el
112
resultado positivo del test realmente significa que está infectado por el virus. Las probabilidades
estaban definidas de la siguiente manera.
𝑃(𝐴 ∩ 𝐵𝑖 )
𝑃(𝐵𝑖 |𝐴) (A.16)
𝑃(𝐴)
Por otro lado para evaluar el denominador P(A) se utiliza la ley de la probabilidad total:
Combinando las expresiones (A.16), (A.17) y (A.18) se llega a la siguiente expresión, conocida
como fórmula de Bayes o teorema de Bayes. Formalmente, siendo 𝐵1 , 𝐵2 , … , 𝐵𝑛 una partición de Ω
y A un suceso. Entonces
𝑃(𝐴|𝐵𝑖 )𝑃(𝐵𝑖 )
𝑃(𝐵𝑖 |𝐴) = ∑𝑛
(A.19)
𝑗=1 𝑃(𝐴|𝐵𝐽 )𝑃(𝐵𝐽 )
113
Volviendo al ejemplo, si deseamos calcular 𝑃(𝐵1 |𝐴), que es la probabilidad de que una persona
esté infectada dado un resultado positivo en el test. Una vez el test ha dado positivo la población se
divide en dos conjuntos, uno denominado 𝐵1 en el que la persona está infectada y otro 𝐵2 en el que
la persona no está infectada. Si usamos la expresión (A.19) que es el teorema de Bayes, tenemos:
𝑃(𝐴|𝐵1 )𝑃(𝐵1 )
𝑃(𝐵1 |𝐴) = (A.20)
𝑃(𝐴|𝐵1 )𝑃(𝐵1 )+𝑃(𝐴|𝐵2 )𝑃(𝐵2 )
Esta manera calcularíamos la probabilidad de que una persona se encuentre infectada sabiendo que
el test ha resultado positivo. Con todos estos conceptos ya tendríamos los suficientes conocimientos
como para comprender la base matemática básica sobre la que se formulan los filtros Bayesianos y
por supuesto los filtros de Kalman. El resto de formulaciones matemáticas relacionadas con los
filtros Bayesianos las encontraremos en el punto 5.4.
114
B
ANEXO B: DATASHEET
Diagrama de Bloques:
115
C.2 OPENCR
Especificaciones:
117
C
ANEXO C:PROGRAMA GMAPPING.CPP
#include "map/GridMap.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include "sensor_msgs/PointCloud2.h"
#include "HectorDrawings.h"
#include "HectorDebugInfoProvider.h"
#include "HectorMapMutex.h"
#ifndef TF_SCALAR_H
typedef btScalar tfScalar;
#endif
HectorMappingRos::HectorMappingRos()
: debugInfoProvider(0)
, hectorDrawings(0)
, lastGetMapUpdateIndex(-100)
, tfB_(0)
, map__publish_thread_(0)
, initial_pose_set_(false)
{
ros::NodeHandle private_nh_("~");
std::string mapTopic_ = "map";
private_nh_.param("pub_drawings", p_pub_drawings, false);
private_nh_.param("pub_debug_output", p_pub_debug_output_, false);
private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true);
private_nh_.param("pub_odometry", p_pub_odometry_,false);
private_nh_.param("advertise_map_service", p_advertise_map_service_,true);
private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5);
private_nh_.param("map_resolution", p_map_resolution_, 0.025);
private_nh_.param("map_size", p_map_size_, 1024);
private_nh_.param("map_start_x", p_map_start_x_, 0.5);
private_nh_.param("map_start_y", p_map_start_y_, 0.5);
private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3);
private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4);
private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9);
private_nh_.param("scan_topic", p_scan_topic_, std::string("scan"));
private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand"));
private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate"));
private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true);
private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false);
private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false);
private_nh_.param("base_frame", p_base_frame_, std::string("base_link"));
private_nh_.param("map_frame", p_map_frame_, std::string("map"));
private_nh_.param("odom_frame", p_odom_frame_, std::string("odom"));
118
private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true);
private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_,
std::string("scanmatcher_frame"));
private_nh_.param("output_timing", p_timing_output_,false);
private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
double tmp = 0.0;
private_nh_.param("laser_min_dist", tmp, 0.4);
p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);
private_nh_.param("laser_max_dist", tmp, 30.0);
p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);
private_nh_.param("laser_z_min_value", tmp, -1.0);
p_laser_z_min_value_ = static_cast<float>(tmp);
private_nh_.param("laser_z_max_value", tmp, 1.0);
p_laser_z_max_value_ = static_cast<float>(tmp);
if (p_pub_drawings)
{
ROS_INFO("HectorSM publishing debug drawings");
hectorDrawings = new HectorDrawings();
}
if(p_pub_debug_output_)
{
ROS_INFO("HectorSM publishing debug info");
debugInfoProvider = new HectorDebugInfoProvider();
}
if(p_pub_odometry_)
{
odometryPublisher_ = node_.advertise<nav_msgs::Odometry>("scanmatch_odom", 50);
}
slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_), p_map_size_,
p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings,
debugInfoProvider);
slamProcessor->setUpdateFactorFree(p_update_factor_free_);
slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);
slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_);
slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);
int mapLevels = slamProcessor->getMapLevels();
mapLevels = 1;
for (int i = 0; i < mapLevels; ++i)
{
mapPubContainer.push_back(MapPublisherContainer());
slamProcessor->addMapMutex(i, new HectorMapMutex());
std::string mapTopicStr(mapTopic_);
if (i != 0)
{
mapTopicStr.append("_" + boost::lexical_cast<std::string>(i));
}
std::string mapMetaTopicStr(mapTopicStr);
mapMetaTopicStr.append("_metadata");
MapPublisherContainer& tmp = mapPubContainer[i];
tmp.mapPublisher_ = node_.advertise<nav_msgs::OccupancyGrid>(mapTopicStr, 1, true);
tmp.mapMetadataPublisher_ = node_.advertise<nav_msgs::MapMetaData>(mapMetaTopicStr, 1, true);
if ( (i == 0) && p_advertise_map_service_)
{
tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this);
}
setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i));
if ( i== 0){
mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
119
}
}
ROS_INFO("HectorSM p_base_frame_: %s", p_base_frame_.c_str());
ROS_INFO("HectorSM p_map_frame_: %s", p_map_frame_.c_str());
ROS_INFO("HectorSM p_odom_frame_: %s", p_odom_frame_.c_str());
ROS_INFO("HectorSM p_scan_topic_: %s", p_scan_topic_.c_str());
ROS_INFO("HectorSM p_use_tf_scan_transformation_: %s", p_use_tf_scan_transformation_ ? ("true") : ("false"));
ROS_INFO("HectorSM p_pub_map_odom_transform_: %s", p_pub_map_odom_transform_ ? ("true") : ("false"));
ROS_INFO("HectorSM p_scan_subscriber_queue_size_: %d", p_scan_subscriber_queue_size_);
ROS_INFO("HectorSM p_map_pub_period_: %f", p_map_pub_period_);
ROS_INFO("HectorSM p_update_factor_free_: %f", p_update_factor_free_);
ROS_INFO("HectorSM p_update_factor_occupied_: %f", p_update_factor_occupied_);
ROS_INFO("HectorSM p_map_update_distance_threshold_: %f ", p_map_update_distance_threshold_);
ROS_INFO("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_);
ROS_INFO("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_);
ROS_INFO("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_);
scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_,
&HectorMappingRos::scanCallback, this);
sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);
poseUpdatePublisher_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>(p_pose_update_topic_, 1,
false);
posePublisher_ = node_.advertise<geometry_msgs::PoseStamped>("slam_out_pose", 1, false);
scan_point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud>("_cloud",1,false);
tfB_ = new tf::TransformBroadcaster();
ROS_ASSERT(tfB_);
/*
bool p_use_static_map_ = false;
if (p_use_static_map_){
mapSubscriber_ = node_.subscribe(mapTopic_, 1, &HectorMappingRos::staticMapCallback, this);
}
*/
initial_pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(node_,
"initialpose", 2);
initial_pose_filter_ = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*initial_pose_sub_, tf_,
p_map_frame_, 2);
initial_pose_filter_->registerCallback(boost::bind(&HectorMappingRos::initialPoseCallback, this, _1));
map__publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this,
p_map_pub_period_));
map_to_odom_.setIdentity();
lastMapPublishTime = ros::Time(0,0);
}
HectorMappingRos::~HectorMappingRos()
{
delete slamProcessor;
if (hectorDrawings)
delete hectorDrawings;
if (debugInfoProvider)
delete debugInfoProvider;
if (tfB_)
delete tfB_;
if(map__publish_thread_)
delete map__publish_thread_;
}
void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
if (hectorDrawings)
{
hectorDrawings->setTime(scan.header.stamp);
120
}
ros::WallTime startTime = ros::WallTime::now();
if (!p_use_tf_scan_transformation_)
{
if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap()))
{
slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
}
}
else
{
ros::Duration dur (0.5);
if (tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur))
{
tf::StampedTransform laserTransform;
tf_.lookupTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp, laserTransform);
//projector_.transformLaserScanToPointCloud(p_base_frame_ ,scan, pointCloud,tf_);
projector_.projectLaser(scan, laser_point_cloud_,30.0);
if (scan_point_cloud_publisher_.getNumSubscribers() > 0){
scan_point_cloud_publisher_.publish(laser_point_cloud_);
}
Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());
if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor-
>getScaleToMap()))
{
if (initial_pose_set_){
initial_pose_set_ = false;
startEstimate = initial_pose_;
}else if (p_use_tf_pose_start_estimate_){
try
{
tf::StampedTransform stamped_pose;
tf_.waitForTransform(p_map_frame_,p_base_frame_, scan.header.stamp, ros::Duration(0.5));
tf_.lookupTransform(p_map_frame_, p_base_frame_, scan.header.stamp, stamped_pose);
tfScalar yaw, pitch, roll;
stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll);
startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
startEstimate = slamProcessor->getLastScanMatchPose();
}
}else{
startEstimate = slamProcessor->getLastScanMatchPose();
}
if (p_map_with_known_poses_){
slamProcessor->update(laserScanContainer, startEstimate, true);
}else{
slamProcessor->update(laserScanContainer, startEstimate);
}
}
else{
ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.",
p_base_frame_.c_str(), scan.header.frame_id.c_str());
return;
}
}
121
if (p_timing_output_)
{
ros::WallDuration duration = ros::WallTime::now() - startTime;
ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f );
}
//If we're just building a map with known poses, we're finished now. Code below this point publishes the localization
results.
if (p_map_with_known_poses_)
{
return;
}
poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(),
scan.header.stamp, p_map_frame_);
poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());
posePublisher_.publish(poseInfoContainer_.getPoseStamped());
if(p_pub_odometry_)
{
nav_msgs::Odometry tmp;
tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
odometryPublisher_.publish(tmp);
}
if (p_pub_map_odom_transform_)
{
tf::StampedTransform odom_to_base;
try
{
tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform failed during publishing of map_odom transform: %s",e.what());
odom_to_base.setIdentity();
}
map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse());
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
}
if (p_pub_map_scanmatch_transform_){
tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp,
p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
}
}
void HectorMappingRos::sysMsgCallback(const std_msgs::String& string)
{
ROS_INFO("HectorSM sysMsgCallback, msg contents: %s", string.data.c_str());
if (string.data == "reset")
{
ROS_INFO("HectorSM reset");
slamProcessor->reset();
}
}
bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request &req,
nav_msgs::GetMap::Response &res)
{
ROS_INFO("HectorSM Map service called");
res = mapPubContainer[0].map_;
return true;
122
}
void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap,
ros::Time timestamp, MapLockerInterface* mapMutex)
{
nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
//only update map if it changed
if (lastGetMapUpdateIndex != gridMap.getUpdateIndex())
{
int sizeX = gridMap.getSizeX();
123
D
ANEXO D: PROGRAMA ULTRASONIDO.INO
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>
ros::NodeHandle nh;
#define echoPin 7 // Echo Pin
#define trigPin 8 // Trigger Pin
int maximumRange = 200; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long duration, distance; // Duration used to calculate distance
sensor_msgs::Range range_msg;
ros::Publisher pub_range( "/ultrasound", &range_msg);
char frameid[] = "/ultrasound";
void setup() {
nh.initNode();
nh.advertise(pub_range);
range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg.header.frame_id = frameid;
range_msg.min_range = 0.0;
range_msg.max_range = 60;
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
float getRange_Ultrasound(){
int val = 0;
for(int i=0; i<4; i++) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
124
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
//Calculate the distance (in cm) based on the speed of sound.
val += duration;
}
return val / 232.8 ;
}
long range_time;
void loop() {
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
if ( millis() >= range_time ){
int r =0;
range_msg.range = getRange_Ultrasound();
range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
E
ANEXO E: PROGRAMA ODOMETRIA Y SENSOR LIDAR
E.1 ODOMETRIA
* Setup
*******************************************************************************/
void setup()
{
// Inicia los nodos de ROS
nh.initNode();
nh.getHardware()->setBaud(115200);
nh.subscribe(cmd_vel_sub);
nh.subscribe(sound_sub);
nh.subscribe(motor_power_sub);
nh.subscribe(reset_sub);
nh.advertise(sensor_state_pub);
nh.advertise(version_info_pub);
nh.advertise(imu_pub);
nh.advertise(cmd_vel_rc100_pub);
nh.advertise(odom_pub);
nh.advertise(joint_states_pub);
nh.advertise(battery_state_pub);
nh.advertise(mag_pub);
tf_broadcaster.init(nh);
// Set Motor
motor_driver.init();
// Set remote controller y cmd_vel
controllers.init(MAX_LINEAR_VELOCITY, MAX_ANGULAR_VELOCITY);
// Set odometry
initOdom();
initJointStates();
prev_update_time = millis();
pinMode(LED_WORKING_CHECK, OUTPUT);
SerialBT2.begin(57600);
setup_end = true;
}
/*******************************************************************************
* Loop
*******************************************************************************/
void loop()
{
uint32_t t = millis();
updateTime();
updateVariable();
tTime[1] = t;
}
// Test de boton
driveTest(diagnosis.getButtonPress());
// Status de Led
diagnosis.showLedStatus(nh.connected());
// Status de Voltaje
battery_state = diagnosis.updateVoltageCheck(setup_end);
nh.spinOnce();
delay(10);
}
/*******************************************************************************
* Funcion cmd_vel msg
*******************************************************************************/
void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg)
{
goal_velocity_from_cmd[LINEAR] = cmd_vel_msg.linear.x;
goal_velocity_from_cmd[ANGULAR] = cmd_vel_msg.angular.z;
/*******************************************************************************
* Funcion motor_power msg
*******************************************************************************/
void motorPowerCallback(const std_msgs::Bool& power_msg)
{
bool dxl_power = power_msg.data;
motor_driver.setTorque(DXL_LEFT_ID, dxl_power);
motor_driver.setTorque(DXL_RIGHT_ID, dxl_power);
}
/*******************************************************************************
* Funcion reset msg
*******************************************************************************/
void resetCallback(const std_msgs::Empty& reset_msg)
{
char log_msg[50];
sensors.calibrationGyro();
initOdom();
/*******************************************************************************
* Publish msgs (CMD Velocity data from RC100 : angular velocity, linear velocity)
*******************************************************************************/
void publishCmdVelFromRC100Msg(void)
{
cmd_vel_rc100_msg.linear.x = goal_velocity_from_rc100[LINEAR];
cmd_vel_rc100_msg.angular.z = goal_velocity_from_rc100[ANGULAR];
cmd_vel_rc100_pub.publish(&cmd_vel_rc100_msg);
}
/*******************************************************************************
* Publish msgs (sensor_state: bumpers, cliffs, buttons, encoders, battery)
*******************************************************************************/
void publishSensorStateMsg(void)
{
bool dxl_comm_result = false;
sensor_state_msg.header.stamp = rosNow();
sensor_state_msg.battery = sensors.checkVoltage();
if (dxl_comm_result == true)
updateMotorInfo(sensor_state_msg.left_encoder, sensor_state_msg.right_encoder);
else
return;
sensor_state_msg.button = sensors.checkPushButton();
sensor_state_msg.torque = motor_driver.getTorque();
sensor_state_pub.publish(&sensor_state_msg);
}
/*******************************************************************************
* Publish msgs (version info)
*******************************************************************************/
void publishVersionInfoMsg(void)
{
version_info_msg.hardware = HARDWARE_VER;
version_info_msg.software = SOFTWARE_VER;
version_info_msg.firmware = FIRMWARE_VER;
version_info_pub.publish(&version_info_msg);
}
/*******************************************************************************
* Publish msgs (battery_state)
*******************************************************************************/
void publishBatteryStateMsg(void)
{
battery_state_msg.header.stamp = rosNow();
battery_state_msg.design_capacity = 1.8f; //Ah
battery_state_msg.voltage = sensors.checkVoltage();
battery_state_msg.percentage = (float)(battery_state_msg.voltage / 11.1f);
if (battery_state == 0)
battery_state_msg.present = false;
else
battery_state_msg.present = true;
battery_state_pub.publish(&battery_state_msg);
}
/*******************************************************************************
* Publish msgs (odometry, joint states, tf)
*******************************************************************************/
void publishDriveInformation(void)
{
unsigned long time_now = millis();
unsigned long step_time = time_now - prev_update_time;
prev_update_time = time_now;
ros::Time stamp_now = rosNow();
128
// calculate odometry
calcOdometry((double)(step_time * 0.001));
// odometry
updateOdometry();
odom.header.stamp = stamp_now;
odom_pub.publish(&odom);
// odometry tf
updateTF(odom_tf);
odom_tf.header.stamp = stamp_now;
tf_broadcaster.sendTransform(odom_tf);
// joint states
updateJointStates();
joint_states.header.stamp = stamp_now;
joint_states_pub.publish(&joint_states);
}
/*******************************************************************************
*Odometry
*******************************************************************************/
void updateOdometry(void)
{
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = odom_pose[0];
odom.pose.pose.position.y = odom_pose[1];
odom.pose.pose.position.z = 0;
odom.pose.pose.orientation = tf::createQuaternionFromYaw(odom_pose[2]);
odom.twist.twist.linear.x = odom_vel[0];
odom.twist.twist.angular.z = odom_vel[2];
}
void updateJointStates(void)
{
static float joint_states_pos[WHEEL_NUM] = {0.0, 0.0};
static float joint_states_vel[WHEEL_NUM] = {0.0, 0.0};
static float joint_states_eff[WHEEL_NUM] = {0.0, 0.0};
joint_states_pos[LEFT] = last_rad[LEFT];
joint_states_pos[RIGHT] = last_rad[RIGHT];
joint_states_vel[LEFT] = last_velocity[LEFT];
joint_states_vel[RIGHT] = last_velocity[RIGHT];
joint_states.position = joint_states_pos;
joint_states.velocity = joint_states_vel;
}
void updateTF(geometry_msgs::TransformStamped& odom_tf)
{
odom_tf.header = odom.header;
odom_tf.child_frame_id = "base_footprint";
odom_tf.transform.translation.x = odom.pose.pose.position.x;
odom_tf.transform.translation.y = odom.pose.pose.position.y;
odom_tf.transform.translation.z = odom.pose.pose.position.z;
odom_tf.transform.rotation = odom.pose.pose.orientation;
}
void updateMotorInfo(int32_t left_tick, int32_t right_tick)
{
int32_t current_tick = 0;
static int32_t last_tick[WHEEL_NUM] = {0.0, 0.0};
if (init_encoder)
{
for (int index = 0; index < WHEEL_NUM; index++)
{
last_diff_tick[index] = 0.0;
last_tick[index] = 0.0;
last_rad[index] = 0.0;
last_velocity[index] = 0.0;
}
last_tick[LEFT] = left_tick;
last_tick[RIGHT] = right_tick;
init_encoder = false;
129
return;
}
current_tick = left_tick;
current_tick = right_tick;
step_time = diff_time;
if (step_time == 0)
return false;
if (isnan(wheel_l))
wheel_l = 0.0;
if (isnan(wheel_r))
wheel_r = 0.0;
v = delta_s / step_time;
w = delta_theta / step_time;
last_theta = theta;
return true;
}
/*******************************************************************************
130
* ros::Time::now() implementation
*******************************************************************************/
ros::Time rosNow()
{
return addMicros(current_time, micros() - current_offset);
}
/*******************************************************************************
* Time Interpolation function
*******************************************************************************/
ros::Time addMicros(ros::Time & t, uint32_t _micros)
{
uint32_t sec, nsec;
sec = _micros / 1000000 + t.sec;
nsec = _micros % 1000000 + 1000 * (t.nsec / 1000);
/*******************************************************************************
* Envio De Msg
*******************************************************************************/
void sendLogMsg(void)
{
static bool log_flag = false;
char log_msg[100];
const char* init_log_data = INIT_LOG_DATA;
if (nh.connected())
{
if (log_flag == false)
{
sprintf(log_msg, "--------------------------");
nh.loginfo(log_msg);
sprintf(log_msg, init_log_data);
nh.loginfo(log_msg);
sprintf(log_msg, "--------------------------");
nh.loginfo(log_msg);
log_flag = true;
}
}
else
{
log_flag = false;
}
}
void initOdom(void)
{
init_encoder = true;
odom.pose.pose.position.x = 0.0;
odom.pose.pose.position.y = 0.0;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation.x = 0.0;
odom.pose.pose.orientation.y = 0.0;
odom.pose.pose.orientation.z = 0.0;
odom.pose.pose.orientation.w = 0.0;
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.angular.z = 0.0;
}
131
void initJointStates(void)
{
static char *joint_states_name[] = {"wheel_left_joint", "wheel_right_joint"};
joint_states.header.frame_id = "base_link";
joint_states.name = joint_states_name;
joint_states.name_length = WHEEL_NUM;
joint_states.position_length = WHEEL_NUM;
joint_states.velocity_length = WHEEL_NUM;
joint_states.effort_length = WHEEL_NUM;
}
#ifndef HLDS_LASER_SEGMENT_PUBLISHER_H_
#define HLDS_LASER_SEGMENT_PUBLISHER_H_
#include <string>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/LaserScan.h>
#include <boost/asio.hpp>
#include <boost/array.hpp>
namespace hls_lfcd_lds
{
class LFCDLaser
{
public:
LFCDLaser(boost::asio::io_service& io);
~LFCDLaser() {};
void poll();
void close();
private:
// ROS NodeHandle
132
ros::NodeHandle nh_;
std::string frame_id_;
sensor_msgs::LaserScan scan_;
std::string port_;
int baud_rate_;
int lfcdstart_;
int lfcdstop_;
int lfcdstartstop_;
bool shutting_down_;
boost::asio::serial_port serial_;
uint16_t motor_speed_;
};
};
#endif