SLAM basado en láser para el robot aéreo Erlecopter

Texto completo

(1)Universidad de Alcalá Escuela Politécnica Superior. MÁSTER UNIVERSITARIO EN INGENIERÍA INDUSTRIAL. Trabajo Fin de Máster. TÍTULO SLAM basado en láser para el robot aéreo Erlecopter. Autor: Nicolás Blanco Fernández Tutor: Rafael Barea Navarro. 2019.

(2)

(3) UNIVERSIDAD DE ALCALÁ Escuela Politécnica Superior. MÁSTER EN INGENIERÍA INDUSTRIAL. Trabajo Fin de Máster. “SLAM basado en láser para el robot aéreo Erlecopter”. Nicolás Blanco Fernández. 2019.

(4) UNIVERSIDAD DE ALCALÁ Escuela Politécnica Superior. MÁSTER EN INGENIERÍA INDUSTRIAL Trabajo Fin de Máster. “SLAM basado en láser para el robot aéreo ErleCopter” Autor:. Nicolás Blanco Fernández. Universidad:. Universidad de Alcalá. País:. España. Profesor Tutor:. Rafael Barea Navarro. Presidente: Felipe Espinosa Zapata. Vocal 1: Pedro Gil Jiménez. Vocal 2: Rafael Barea Navarro. CALIFICACIÓN:......................................................................... FECHA:.........................................................................................

(5) Agradecimientos. El agradecimiento más profundo y sentido va para mi familia. En especial, a dos grandes pilares: mis abuelas Carmen y Toñi. Todo un ejemplo de vida y superación, con espíritu luchador y gran capacidad de mantener unida a la familia. Gracias por criarme, cuidarme y enseñarme esas cosas que no se aprenden en la escuela ni en la universidad, sino en casa de la abuela escuchando esas historias, que quizás, sin ser vosotras conscientes, hoy se han convertido en lecciones de vida. Simplemente quería deciros que vuestro nieto está muy orgulloso de vosotras, teneros es todo un regalo. Para mi siempre seréis eternas.. También quiero acordarme de mis padres Nicolás y Pilar, mi hermana Lucía y mi novia Alba. A cuatro personas fundamentales en la guía de mi vida, mi más sincero agradecimiento, sin su apoyo, colaboración y ánimo habría sido imposible llegar hasta aquí. Por último, quiero dar las gracias a mis amigos, compañeros y profesores tanto de la Universidad de Alcalá como de la Mälardalens University el permitirme vivir esta gran experiencia. En especial a mi tutor Rafael Barea y Elena López por su motivación y entrega en el desarrollo de este proyecto, así como por abrirme de esta forma los ojos en este gran mundo de la robótica..

(6) Contenido general Resumen .................................................................................................................................. 9 Abstract .................................................................................................................................. 11 Resumen extendido ................................................................................................................ 13 Extended Abstract .................................................................................................................. 15 Lista de figuras ...................................................................................................................... 17 Lista de tablas ........................................................................................................................ 21 Memoria................................................................................................................................. 24 1 Introducción ........................................................................................................................ 24 1.1 Antecedentes ............................................................................................................... 24 1.2 Objetivos del proyecto.................................................................................................. 26 1.3 Estructura de la memoria ............................................................................................. 27 2 Herramientas utilizadas ....................................................................................................... 29 2.1 ErleCopter .................................................................................................................... 29 2.1.1 Características Hardware ....................................................................................... 30 2.1.2 Modos de vuelo ..................................................................................................... 34 2.2 Robot Operating System (ROS) ..................................................................................... 35 2.2.1 Sistema de archivos ROS ........................................................................................ 36 2.2.2 Sistema de paquetes ROS ...................................................................................... 37 2.2.3 Niveles de computación en ROS ............................................................................. 38 2.3 Herramientas de visualización y simuladores ................................................................ 41 2.3.1 Gazebo .................................................................................................................. 41 2.3.2 rqt ......................................................................................................................... 42 2.3.3 Rviz ........................................................................................................................ 43 2.3.4 APM Planner .......................................................................................................... 44 3 Arquitectura general del sistema ......................................................................................... 46 3.1 Arquitectura HW y SW en plataforma real .................................................................... 46 3.2 Arquitectura HW y SW en plataforma simulada ............................................................ 48 4 SLAM: Hector mapping ........................................................................................................ 49 4.1 Hector mapping ............................................................................................................ 49 4.1.1 Acceso al mapa ...................................................................................................... 50 6.

(7) 4.1.2 Scan matching ....................................................................................................... 52 4.2 Implementación de Hector mapping ............................................................................. 54 4.3 Implementación de Hector mapping en robot real ........................................................ 60 4.4 Resultados de Hector mapping en robot real ................................................................. 63 5 Fusión de datos con EKF ...................................................................................................... 68 5.1 Filtro de Kalman ........................................................................................................... 68 5.2 Filtro Extendido de Kalman ........................................................................................... 69 6 Controlador PID .................................................................................................................. 74 6.1 Implementación del controlador PID ............................................................................ 74 6.2 Algoritmos PID desarrollados ........................................................................................ 77 6.3 Resultados del controlador PID ..................................................................................... 79 7 Resultados .......................................................................................................................... 82 7.1 Resultados 1 ................................................................................................................. 83 7.2 Resultados 2 ................................................................................................................. 86 7.3 Resultados 3 ................................................................................................................. 91 7.4 Resultados 4 ................................................................................................................. 92 7.5 Conclusiones ................................................................................................................ 95 8 Conclusiones y trabajo futuros ............................................................................................ 96 Manual de usuario ................................................................................................................. 98 M.1 Tutorial lanzamiento de la simulación: ........................................................................ 98 Pliego de condiciones ........................................................................................................... 102 PL.1 Hardware.................................................................................................................. 102 PL.2 Software ................................................................................................................... 102 Planos .................................................................................................................................. 104 Presupuesto.......................................................................................................................... 114 PR.1 Costes materiales ..................................................................................................... 114 PR.2 Costes profesionales................................................................................................. 115 PR.3 Costes totales .......................................................................................................... 115 Bibliografía .......................................................................................................................... 116. 7.

(8) 8.

(9) Resumen El objetivo de este Proyecto es desarrollar e implementar un sistema de Localización y Mapeado Simultáneo (SLAM) para el robot aéreo Erlecopter, basado en un láser como sensor principal. Para ello se hace uso del algoritmo de scan matching “Hector-mapping”. Para la estimación de la posición del robot aéreo, la información obtenida mediante dicho algoritmo, es fusionada con otras medidas del resto de sensores a bordo del robot como los ultrasonidos instalados para obtener información a cerca de la altura. Para dicha fusión de datos se ha desarrollado un Filtro de Kalman Extendido. Como entorno de desarrollo se utiliza ROS (Robot Operating System), junto a Gazebo y Rviz como herramientas de simulación y visualización respectivamente.. Palabras clave: MAVs (Micro Aereal Vehicles), drones, EKF (Filtro de Kalman Extendido), SLAM (Localización y Mapeado Simultáneo), Hector-mapping.. 9.

(10) 10.

(11) Abstract The aim of this Project is to develop and implement a Simultaneous Location and Mapping (SLAM) for the aereal robot Erlecpter. This project is based on a laser as principal sensor. For this proposal, it is used the algorithm of scan matching “Hectormapping”. In order to calculate the position of the aereal robot, the information obtained through this algorithm is merged with the rest of the sensors on the robot, such as for instance, the ultrasounds that are installed with the aim of gather data related with the altitude. For this merging of data, it has been implemented an Extended Kalman Filter. The development environment used is ROS (Robot Operating System) together with Gazebo and Rviz, both as tools of simulation and visualization respectively.. Keywords: MAVs (Micro Aereal Vehicles), drones, EKF (Extended Kalman Filter), SLAM (Simultaneous Localization and Mapping), Hector-mapping.. 11.

(12) 12.

(13) Resumen extendido Este Trabajo Fin de Máster parte del mundo de la robótica, en especial de los MAVs (Micro Aereal Vehicles) conocidos comúnmente como drones. Estos dispositivos proporcionan una gran diversidad de aplicaciones en la actualidad tales como vigilancia aérea, desplazamiento rápido en terrenos irregulares de difícil accesibilidad obteniendo incluso una vista de pájaro del entorno, lo que lo convierte en una aplicación de especial utilidad como apoyo a equipos de rescate y proyectos militares, entre otros. Muchos de estos MAVs están dotados de un pequeño ordenador que les hace especialmente útiles a la hora de desarrollar aplicaciones que requieren una mayor autonomía, permitiendo incluso la toma de decisiones por ellos mismos según las condiciones de su entorno. Un ejemplo de ello es el robot aéreo tomado para el desarrollo de este proyecto: el ErleCopter, dotado de un controlador Erle-Brain2, basado en una Raspberry Pi con ROS instalado y el autopiloto APM, donde se podrán ejecutar los distintos algoritmos en tiempo real. El trabajo surge de la importancia de profundizar en el conocimiento del control de robots aéreos y la capacidad de que estos puedan desplazarse de manera autónoma, haciéndoles aptos para obtener su posición y un mapa de su entorno mediante un sensor láser (Láser Hokuyo URG-04LX) y sensores ultrasonidos, cuando dicho entorno tenga unas condiciones que no permiten el posicionamiento mediante GPS. Las medidas obtenidas mediante el algoritmo de scan matching “Hector-mapping” (el cual trata los datos generados por el láser), junto a los de los ultrasonidos, son fusionados mediante un Filtro de Kalman Extendido (EKF), y gracias a la creencia de 13.

(14) posicionamiento que se consigue de éste, se tendrán un seguimiento de la posición del robot fiel a la realidad. Para el control del MAV, se ha desarrollado un controlador PID, logrando que realice una trayectoria definida para que, observando lo que le rodea, sea capaz de crear un mapa del entorno del robot. Todo el trabajo se realizará mediante el entorno de desarrollo ROS, sobre C++. También se empleará la herramienta Matlab como ayuda para la comprobación de datos y la obtención de resultados. Finalmente se expondrán las conclusiones adquiridas, así como las futuras aplicaciones y trabajos derivados del mismo.. 14.

(15) Extended Abstract This Final Master Project arises from the field of the robotic, specially the MAVs (Micro Aereal Vehicles), which are commonly known as drones. These devices provide a great diversity of applies in the present, such as for example, aereal surveillance, fast movements by irregular grounds which are difficult to access them, even with the capability of obtain a bird view of the environment, very useful for rescue crew and militar projects, among others. Many of these MAVs, have a microcontroller which enables to be especially useful to develop applications that require a major autonomy, because it allows them to take decisions by itself according to the conditions of the environment. A good example of this, is the aereal robot used in this case: the ErleCopter, equipped with a controller ErleBrain2, based on a Raspberry Pi with ROS installed and the autopilot APM, where is possible to execute the different algorithms on real time. This Project arises from the importance of deepening in the knowledge of the control of aereal robots and the capability of them to move with autonomy. This fact makes them suitable for obtain their position and a map of the environment through a laser sensor (Laser Hokuyo URG-04LX) and ultrasound sensors, in the case of that the environment has some conditions which hamper its positioning with GPS. The measures obtained with the algorithm of scan matching “Hector-mapping” (which use the data provided by the laser) together with the ultrasounds, all of them are merged through an Extended Kalman Filter (EKF), and thanks to the belief of the positioning that it is got from this, we can obtain a follow-up of the realistic position of the robot. 15.

(16) In order to the control of the MAV, it has been developed a controller PID, achieving the fact that the robot executes a defined path observing everything that surrounds it, and being capable of create a map of the environment of it. The development environment used along all the process is ROS, based on C++. Besides, it uses the software Matlab as a help tool to check the data and the results collection. Finally, it will be explained the conclusions, which have been reached, as well as the future applications and possible projects derivate from this.. 16.

(17) Lista de figuras. Figura 1.1. Pop.Up Next: un taxi eléctrico, volador y autónomo de Audi y Airbus ......... 25. Figura 1.2. Shield AI drone ........................................................................................... 25. Figura 2.1. Erle-Copter .................................................................................................. 29. Figura 2.2. Componenetes principales Erlecopter .......................................................... 30. Figura 2.3. Erle-Brain 2 ................................................................................................. 31. Figura 2.4. Controlador Eléctrico de Velocidad ............................................................. 32. Figura 2.5. Motores DC del Erlecopter .......................................................................... 32. Figura 2.6. Módulo de potencia ..................................................................................... 33. Figura 2.7. Láser Hokuyo URG-04LX ........................................................................... 33. Figura 2.8. Ultrasonidos MB1030 .................................................................................. 34. Figura 2.9. Niveles de los archivos de sistema en ROS .................................................. 36. Figura 2.10 Estructura típica de un paquete en ROS ....................................................... 37 Figura 2.11. Estructura de las capas en ROS ................................................................... 38. Figura 2.12 Gazebo ........................................................................................................ 41 Figura 2.13 rqt ............................................................................................................... 42 Figura 2.14 RViz ........................................................................................................... 43 Figura 2.15 Datos de vuelo APM Planner ....................................................................... 44 Figura 2.16 Armado y configuración de modo en APM Planner ..................................... 45. 17.

(18) Figura 3.1. Arquitectura general del sistema .................................................................. 47. Figura 4.1. Mapa de cuadrícula 2D de hector_mapping y láser (Rviz) ............................ 50. Figura 4.2. Representación del mapa (cuadrícula ocupada) ............................................ 52. Figura 4.3. Predicción de la posición (derivadas en el espacio) ...................................... 52. Figura 4.4. Transformaciones de Hector Mapping ......................................................... 55. Figura 4.5. TFs estáticas de los sensores (rqt) ................................................................ 56. Figura 4.6. TFs tras ejecutar hector_mapping por defecto (rqt) ...................................... 57. Figura 4.7. TFs tras configurar hector_mapping (Rviz) .................................................. 58. Figura 4.8. Nodos/topics activos tras configurar hector_mapping ................................... 59. Figura 4.9. Visualización de mapa generado por hector_mapping (Rviz) ....................... 60. Figura 4.10 TFs tras configurar hector_mapping en Erlecopter real (Rviz) ...................... 62 Figura 4.11 Nodos/topics activos tras configurar hector_mapping en Erlecopter real ...... 63 Figura 4.12 Mapa 1 generado por Erlecopter real (Rviz) ................................................. 64 Figura 4.13 Mapa 2 generado por Erlecopter real (Rviz) ................................................. 65 Figura 4.14 Mapa 3 generado por Erlecopter real (Rviz) ................................................. 66 Figura 5.1. Esquema de coordenadas locales del Erlecopter ........................................... 70. Figura 6.1. Bloque de diagramas de un controlador PID ................................................ 75. Figura 6.2. Gráfico de nodos/topics activos en pid (rqt) ................................................. 77. Figura 6.3. Gráfico modelo PID implementado ............................................................. 78. Figura 6.4. Gráfico de nodos/topics activos en pid_ekf .................................................. 79. Figura 6.5. x(t) y RC_pitch(t) ........................................................................................ 80. Figura 6.6. y(t) y RC_roll(t) .......................................................................................... 80. Figura 6.7. Gráfico modelo PID implementado .............................................................. 81. Figura 7.1. Sistema de bloques del proyecto .................................................................. 82. Figura 7.2. Mundo1 Gazebo (cuadrado) ......................................................................... 83. Figura 7.3. Mapa obtenido mediante hector_mapping en Rviz (Cuadrado) ..................... 83. Figura 7.4. Posición de Erlecopter cuadrado: real vs estimada por EKF (3D, Matlab) .... 84. Figura 7.5. Posición de Erlecopter cuadrado: real vs estimada por EKF (xy, Matlab) ...... 84. Figura 7.6. Posición de Erlecopter cuadrado: real vs estimada por EKF (xy, Matlab) ..... 85 18.

(19) Figura 7.7. Posición de Erlecopter cuadrado: real vs estimada por EKF (xz, Matlab) ..... 85. Figura 7.8. Posición de Erlecopter cuadrado: real vs estimada por EKF (yz, Matlab) ..... 86. Figura 7.9. Mundo1 a evaluar (Gazebo) ......................................................................... 87. Figura 7.10. Mapa obtenido del Mundo1 (Rviz) ............................................................. 87. Figura 7.11. Árbol de TFs final (Rviz) ............................................................................ 88. Figura 7.12. Gráfico de nodos/topics activos final (Rviz) ................................................ 88. Figura 7.13. Posición de Erlecopter Mundo1: real vs estimada por EKF (3D, Matlab) ..... 89. Figura 7.14. Posición de Erlecopter Mundo1: real vs estimada por EKF (xy, Matlab) ..... 89. Figura 7.15. Posición de Erlecopter Mundo1: real vs estimada por EKF (xz, Matlab) ..... 90. Figura 7.16. Posición de Erlecopter Mundo1: real vs estimada por EKF (yz, Matlab) ..... 90. Figura 7.17. Mundo2 Pasillos lisos (Gazebo) .................................................................. 91. Figura 7.18. Limitación en la generación del mapa en superficies uniformes (Rviz) ........ 91. Figura 7.19. Mundo2 a evaluar (Gazebo) ........................................................................ 92. Figura 7.20. Mapa obtenido del Mundo2 (Rviz) ............................................................. 92. Figura 7.21. Posición de Erlecopter Mundo2: real vs estimada por EKF (3D, Matlab) .... 93. Figura 7.22. Posición de Erlecopter Mundo2: real vs estimada por EKF (xy, Matlab) ..... 93. Figura 7.23. Posición de Erlecopter Mundo2: real vs estimada por EKF (xz, Matlab) ..... 94. Figura 7.24. Posición de Erlecopter Mundo2: real vs estimada por EKF (yz, Matlab) ..... 94. Figura P.1. APM Planner2 simulación ......................................................................... 109. Figura P.2. APM Planner2 entorno real ....................................................................... 110. Figura P.3. Características Láser Hokuyo URG-04LX ................................................. 112. Figura P.4. Características Ultrasonidos MB1030 LV-MaxSonar-EZ3 ......................... 113. 19.

(20) 20.

(21) Lista de tablas. TABLA 4.1. Sensores, TFs y topics ............................................................................... 55. TABLA 6.1. Ganancias PID .......................................................................................... 81. TABLA PR.1 Costes materiales (hardware y software) .................................................. 114 TABLA PR.2 Costes profesionales ................................................................................ 115 TABLA PR.3 Costes totales IVA incluido ..................................................................... 115. 21.

(22) 22.

(23) 23.

(24) SLAM basado en láser para el robot aéreo ErleCopter. Memoria. 1 Introducción 1.1 Antecedentes El dominio del ser humano sobre la tecnología crece a pasos agigantados, de tal forma que se avanza hacia una automatización global. La robótica es uno de los segmentos de la tecnología que, sin duda, mayor crecimiento ha experimentado en la última década y que más expectativas de desarrollo muestra. El desarrollo de los drones en los últimos años está sufriendo, sin duda, un progreso exponencial tanto a nivel de vehículos comerciales como no comerciales. Más si cabe, la implementación de algoritmos para permitir la navegación autónoma de estas aeronaves, donde grandes compañías apuestan por la investigación en esta área. Un ejemplo de ello es la propuesta de un taxi aéreo autónomo, desarrollado por las compañías Audi, Italdesing y Airbus.. 24.

(25) Introducción. FIGURA 1.1 Pop.Up Next: un taxi eléctrico, volador y autónomo de Audi y Airbus. Se debe tener también en cuenta, el gran desarrollo de estos dispositivos destinado al uso militar. Otro es el ejemplo de la empresa estadounidense Shield AI, la cual desarrolla microdrones autónomos para ser empleados en zonas urbanas con finalidades de vigilancia, haciendo uso de intligencia artificial.. FIGURA 1.2 Shield AI drone. 25.

(26) SLAM basado en láser para el robot aéreo ErleCopter. Dada la gran apuesta que se hace por esta tecnología, y el largo desarrollo que aún queda, son las universidades las que están en primera linea dedicándose de pleno a la investigación de esta. Son varios los proyectos de la Universidad de Alcalcá destinados a tal investigación, aquí se enumeran algunos de gran interés: •. “Desarrollo de un sistema de SLAM para el robot AR.Drone en entorno ROS” por Álvaro Andrés Saltos Vásquez (2015). •. “Visual SLAM Algorithms for Aerial Robots” por Serio García Gonzalo (2017). •. “Vision-based SLAM for the aerial robot ErleCopter” por Guillermo Patiño González (2018).. 1.2 Objetivos del proyecto El proyecto surge de la importancia de profundizar en el conocimiento de los sistemas aéreos no tripulados, así como su posicionamiento en tiempo real en un entorno desconocido para el robot. A continuación, se muestra un listado con los objetivos específicos de mayor relevancia desarrollados en este trabajo: •. Estudio inicial del robot Erlecopter. Aquí se incluyen tareas de configuración y familiarización con el entorno de trabajo y estdio de los distintos modos de vuelo y comandos específicos que presenta este robot.. •. Conexión robot real – ordenador remoto, así como instalación de sensores GPS, Láser Hokuyo y ultrasonidos y adquisición y almacenamiento de los datos generados por estos.. •. Desarrollo de un sistema de control a partir de datos de posicionamiento proporcionados mediante la simulación en Gazebo, permitiendo enviar al robot aéreo a posiciones de destino determinadas. Esto posibilitará la obtención de información de los sensores en simulación.. •. Realización de la fusión mediante un filtro de Kalman extendido, de la estimación de la posición a partir de los datos obtenidos mediante Hector-Mapping y los 26.

(27) Introducción. ultrasonidos, y comprobación de las mejoras logradas en el resultado de la estimación. •. Desarrollo de un sistema de control a partir de datos de posicionamiento basado en la estimación de posición obtenida a través del filtro de Kalman, permitiendo enviar al robot aéreo a posiciones de destino determinadas,. Esta orientación del proyecto permitirá a futuro el desarrollo de nuevos algoritmos, que junto a la información generada por otros sensores como puedan ser GPS o cámara de visión, puedan ser añadidos en la fusión de datos mediante el EKF, para que la localización y mapeado del entorno sean mucho más robustos ante perturbaciones o acciones externas.. 1.3 Estructura de la memoria A continuación, se muestra una breve estructura de la memoria de trabajo. El primer apartado (capítulo actual) presenta la introducción, muestra los antecedentes y los objetivos del proyecto. El segundo apartado describe detalladamente cada una de las herramientas que se van a emplear: ErleCopter, ROS y algunos paquetes especiales como Gazebo (entorno de simulación), rqt o Rviz. El tercer apartado se presenta una estructura de la arquitectura general del sistema tanto hardware como software. El cuarto apartado recoge información sobre Hector mapping, incluyendo los algoritmos de scan matching empleados y la implementación de estos tanto en simulación como en el robot real. El quinto apartado expone la implementación del Filtro de Kalman Extendido, presentando las ecuaciones obtenidas de los modelos de predición y corrección. En el sexto apartado se explica la implementación de los controladoress PID desarrollado, controlando el posicionamieno del ErleCopter tanto a partir de datos generados por el propio simulador Gazebo como a partir de los datos obtenidos por el EKF desarrollado.. 27.

(28) SLAM basado en láser para el robot aéreo ErleCopter. El séptimo apartado muestra los resultados finales tras lanzar todos los algoritmos desarrollados, incluyendo el EKF y el PID para realizar una trayectoria definida a partir de la información generada por el EKF. El octavo apartado indica las principales conclusiones sobre el sistema realizado y se proponen diferentes líneas de trabajo futuro. Por último, se incluyen los apartados de manual de usuario, plieo de condiciones, planos y presupuesto.. 28.

(29) Herramientas utilizadas. 2 Herramientas utilizadas En este apartado se muestra un breve resumen acerca de las herramientas, tanto software como hardware, empleadas para la realización de la aplicación.. 2.1 ErleCopter Erle-Copter1 es un cuadricóptero desarrollado por la marca española Erle Robotics. Es el primer multirrotor volador basado en Linux que emplea una plataforma robótica como ROS y el autopiloto automático del software APM para lograr distintos modos de vuelo. El robot está diseñado idealmente para operaciones outdoor, siendo capaz de levantar hasta 2 Kg de peso. A pesar de ello, el robot es ideal para lograr la finalidad de este proyecto, ya que el objetivo principal es el desarrollo de un algoritmo SLAM que permita la generación del mapa del entorno del robot en tiempo real, mediante la fusión de los datos generados por los distintos sensores de abordo, así como un control de los distintos modos de vuelo. El hecho estar dotado este vehículo de Erle-Brain 2, un sistema abierto basado en Linux, que permite la conexión de otros sensores, además de los ya integrados, posibilita en gran medida este objetivo.. FIGURA 2.1 Erle-Copter. 1. http://docs.erlerobotics.com/erle_robots/erle_copter. 29.

(30) SLAM basado en láser para el robot aéreo ErleCopter. Adicionalmente, cuenta con prototipos de simulación predefinidos, útiles para llevar el estudio a cabo.. 2.1.1 Características Hardware En cuanto a la equipamenta hardware, este robot dispone la siguiente estructura hardware:. FIGURA 2.2 Componenetes principales Erlecopter. A continuación, se detallan explícitamtne los más importantes: •. Erle-Brain 2: es la segundageneración de cerebro robótico basado en Linux para drones y otros robots con soporte oficial para ROS. Está basado en una Raspberry Pi con ROS y el autopiloto APM. Se encuentra dotado de una amplia variedad de mecanismos de conexión como puertos I2C o UART, lo que posibilita la adición de una amplia gama de sensores externos. También posee tarjetas de comunicación Ethernet, puerto USB, puerto HDMI, conector jack o lector de tarjectas microSD.. 30.

(31) Herramientas utilizadas. FIGURA 2.3 Erle-Brain 2. El procesador consta de una CPU de 900MHz quad-core ARM Cortex-A7 y 1GB RAM. En cuanto a los sensores integrados, dispone de sensor de gravedad, giróscopo, bújula digital, sensor de presión, sensor de temperatura, cámara de visión de 5Mp y WiFi. Erle-Brain 2 también dispone de 12 canales para salidas PWM, cada uno con capacidad de hasta 25mA a 5V. Los pines situados en la izquierda, indicados en el esquema anterior como PPM-Sum input chanel, están destinados a la entrada de la señal de Radio Control.. 31.

(32) SLAM basado en láser para el robot aéreo ErleCopter. •. Baterías de Litio y Polímero: dado que se considera aceptable disponer de 1000 mAh por motor, se dispone de una batería de 4000mAh constituida por diferentes celdas.. •. ESC (Controlador Electrónico de Velocidad): consta de un circuito electrónico el cual permite variar la velocidad de un servomotor proporcionando una corriente trifásica de bajo voltaje para energizar el motor.. FIGURA 2.4 Controlador Eléctrico de Velocidad. •. Motores: 4 motores DC sin escobillas, debido a las mejores prestaciones que ofrecen sobre los que tienen escobillas (fiabilidad, reducción de ruido, mayor eficiencia, mayor reducción de interferencias electromagnéticas o mayor par por peso entre otras). Son motores síncronos que utilizan una fuente de coriente continua, que a través de un inversor (ESC) produce una señal de corriente alterna para controlar el motor.. FIGURA 2.5 Motores DC del Erlecopter. 32.

(33) Herramientas utilizadas. •. Módulo de potencia: permite la alimentación del autopiloto y los periféricos, además de informar sobre el voltaje y la corriente de la batería. Tiene un autoregulador de salida de 5.3V y soporta un máximo de 2.25A.. FIGURA 2.6 Módulo de potencia. •. Receptor de radio frecuencia: es la parte intermedia entre el mando y el Erle-Brain 2 que posibilita la teleoperación. Este recibe las señales de radio control emitidas por el mando y los envía al Erle-Brain mediante codificación PPM.. •. GPS: es un periférico adicional que proporciona mediante un sistema de navegación por satélite información de localización en exteriores bajo cualquier condición climática.. •. Láser Hokuyo URG-04LX: periférico adicional conectado al ErleBrain mediante interfaz USB que permite hacer un escáner láser 2D del entorno con una precisión de 10mm y rango de 240º, midiendo distancias de 0.06 a 4.095 metros. La frecuencia de escaneo es llevada a cabo cada 10 Hz con una resolución angular de 0.36º y su peso de 160g aproximadamente.. FIGURA 2.7 Láser Hokuyo URG-04LX. 33.

(34) SLAM basado en láser para el robot aéreo ErleCopter. •. Ultrasonidos MB1030 LV-MaxSonar-EZ3: periféricos adicionales conectados al Erle_Brain mediante interfaz I2C. Permiten detectar la proximdiad de las superficies inferior y superior al ErleCopter (suelo y techo) a una distancia máxima de 6 metros.. FIGURA 2.8 Ultrasonidos MB1030. 2.1.2 Modos de vuelo Existen distintos modos de vuelo para el Erle-Copter caracterizados para distintas funcionalidades. Dado que la finalidad de esta aplicación va destinada al uso en interiores, haré distinción entre aquellos modos que precisen o no de GPS: Modos que no requieren de sensor GPS: •. Sabilize: permite un vuelo del vehículo manual, pero autonoivela los niveles de roll y pitch.. •. Alt Hold: mantiene una altitud constante, permitiendo un control en roll, pitch y yaw.. •. Acro: permite un control de la velocidad angular mediante RC (recomendable para acrobacias o controles rápidos). •. Land: posibilita un aterrizaje del cóptero en tierra.. Modos que requieren de sensor GPS: •. Loiter: mantiene la misma posición: los grados y la altitud. 34.

(35) Herramientas utilizadas. •. RTL (Return to Launch): el cóptero vuelve desde su actual posición hasta su lugar de lanzamiento.. •. Guided: permite el guiado a una localización usando un módulo radio de telemetría y una aplicación de estación de tierra.. •. Drift: posibilita al usuario un vuelo como si fuera un avión, en tiempo automáticos coordinados.. •. PosHold: es similar al modo Loiter, pero añadiendo un control sobre el piloto del throttle.. •. Follow Me: permite seguir tus movimientos usando un módulo radio de telemetría y una aplicación de estación de tierra como Mission Planner.. •. Circle: logra una trayectoría circular del drone sobre un punto manteniendo la cara mirando al centro.. 2.2 Robot Operating System (ROS) ROS2, el sistema operativo para robots bajo licencia de código abierto, es una plataforma de gran flexibilidad para desarrollar software de robot, simplificando la tarea de crear un comportamiento robótico complejo y robusto en una amplia variedad de plataformas robóticas. Está provisto de librerías y herraminetas, además de abstracción de hardware, controladores, herramientas de visualización, la transferencia de mensajes entre procesos y la administración de paquetes entre otros para facilitar el desarrollo de las aplicaciones. ROS actualmente sólo se ejecuta en plataformas basadas en Unix, probándose principlamente en los sitemas Ubunto y Mac OS X. También ha de tenerse en cuenta que hay una comunidad de desarrolladores activa que da gran respaldo a muchas de las herramientas de esta plataforma. Existen paquetes desarrollados, como algoritmos SLAM, entre muchos otros, de gran versatilidad y configurabilidad que pueden ser directamente implementados en el software de esta aplicación, de hecho, no tendría mucho sentido volver a desarrollarlos ya que son su mejor modo de implementación.. 2. http://www.ros.org/. 35.

(36) SLAM basado en láser para el robot aéreo ErleCopter. 2.2.1 Sistema de archivos ROS De forma similar a un sistema operativo, los archivos de ROS se estructuran de un modo específico en el disco duro. El gráfico expuesto a continuación lo muestra:. FIGURA 2.8 Niveles de los archivos de sistema en ROS. A continuación, se explican cada uno de ellos: •. Packages (Paquetes): son la unidad más basica de un programa en ROS. Contiene los procesos de ejecución (nodos), librerías y configuración de archivos organizados como una sóla unidad. Son el elmento de lanzamiento en el software ROS. Un paquete puede contener múltiples nodos, ficheros de configuración, etc. destinados al logro de una aplicación específica.. •. Package manifest (Paquete manifiesto): es el archivo package.xml dentro del paquete ROS. Contine información sobre este, autor, licencia, dependencias, marcas de compilación.... •. Meta packages (Meta paquetes): es un conjunto de paquetes con un propóstico concreto. Un ejemplo es la pila de navegación de ROS.. •. Meta packages manifest (Manifiesto de Meta paquetes): es similar al Package manifest, sólo que podría incluir dependencias de tiempo de ejecución y declaraciones de variables de exportación. 36.

(37) Herramientas utilizadas. •. Messages (Mensajes): son un tipo de información con extensión .msg que se envía de un proceso de ROS a otro. Estos son definidos en (ejemplo_paquete / msg / ejemplo_mensaje.msg).. •. Services (Servicios): son un tipo de comunicación solicitud / respuesta entre procesos. Estos son definidos en (ejemplo_paquete / srv / ejemplo_servicio.msg).. •. Repositories (Repositorios): gran parte de los paquetes de ROS se mantienen mediante un control de versiones como Git. El conjunto de paquetes que comparten una misma versión pueden denominarse repositorios.. 2.2.2 Sistema de paquetes ROS Una estructura típica de paquetes en ROS es la siguiente:. FIGURA 2.9 Estructura típica de un paquete en ROS. A continuación, se resume la finalidad de cada carpeta: •. config: consta de los archivos de configuración del paquete.. •. include: incluye encabezados y librerías empleados dentro del paquete.. •. scripts: almacena los ejecutables en Python.. •. src: contiene los códigos fuente de C++.. •. launch: mantiene los archivos de inicio que se emplean para iniciar uno o más nodos.. •. msg: incluye la definición de los mensajes.. •. srv: incluye la definición de los servicios. 37.

(38) SLAM basado en láser para el robot aéreo ErleCopter. •. action: incluye la definción de la acción.. •. package.xml: es el paquete manifiesto del paquete.. •. CmakeLists.txt: es el archivo de compilación Cmake del paquete.. 2.2.3 Niveles de computación en ROS La computación en ROS se lleva a cabo mediente una red de procesos denominados nodos.. FIGURA 2.10 Estructura de las capas en ROS. A continuación, se detallan los conceptos principales de computación: •. Nodos: como se ha indicado previamente, estos son los que llevan a cabo la computación del proceso. Cada nodo se escribe empleando librerías como roscpp y rospy. En un robot habrá muchos nodos para ejecutar distintos tipos de tareas, pudiendo comunicarse entre sí e intercambiar datos mediante “topics” y “servicios”. De este modo se consigue una estructura simple y fácil de depurar. Algunas instrucciones de utilidad son: o rosnode list: devuelve la lista de nodos activos. 38.

(39) Herramientas utilizadas. o rosnode info “nombre_nodo”: devuelve información de ese nodo (publicaciones, suscripciones, servicios que ofrece y conexiones con el resto de nodos). o rosnode kill “nombre_nodo”: finaliza la ejecución del nodo. •. Master: el maestro de ROS permite la coordinación, sincronización y búsqueda del sistema de nodos, topics y servicios de ROS. En un sistema distribuido, este debe ser ejecutado en una computadora, permitiendo a nodos remotos comuicarse entre ellos a través de este maestro. Se ejecuta mediante la instrucción roscore, aunque si algún programa se ejecuta mediante la instruccion roslaunch, el master es lanzado automáticamente.. •. Parameter Server: es la ubicación central del Master en que se almacenan los datos para que los nodos puedan acceder a la lectura y modificación de estos.. •. Messages: son una estructura simple de datos que permite a los nodos comunicarse entre sí. Hay tipos estándar (enteros, coma flotante, booleanos...) pudiendo crearte tus propios tipos de mensajes empleando estos tipos estándar.. •. Topics: son canales de comunicación que emplean un tipo de mensaje determinado para ser enviado entre nodos. Cuando un nodo envía un mensaje a través de un topic, se dice que está publicando un topic, mientras que si lo recibe se dice que se está suscribiendo. Cada topic tiene un nombre único y cualquier nodo puede publicar o suscribirse a este simpre y cuando tenga el mismo tipo de mensaje. Debe tenerse en cuenta que el tipo de mensaje es único, por lo que si se desea transmitir varios tipos de datos entre dos nodos, debe crearse un topic para cada uno. Algunas instrucciones de utilidad son: o rostopic list: devuelve la lista de los topics activos. o rostopic info “nombre_topic”: da información sobre el tipo de dato que transmite el topic indicado, así como los nodos que publica y a los que está suscrito. o rostopic type “nombre_topic”: da información sobre el tipo de dato que se envía a través de ese topic. o rostopic echo “nombre_topic”: se suscribe al topic indicado, volcando en la terminal los datos que se están transmitiendo en el topic indicado. o rostopic pub “topic” “tipo_dato” “datos”: publica datos en un topic desde la terminal. 39.

(40) SLAM basado en láser para el robot aéreo ErleCopter. •. Servicios: en determinadas aplicaciones no es suficiente un modelo de comunicación unidireccional de publicación/suscripción. Los servicios posibilitan otra comunicación entre nodos de tipo solicitud/respuesta, empleado en gran medida en sistemas distribuidos, distinguiendo así un nodo servidor y otro cliente. El servidor proporciona el servicio bajo un nombre, cuando el cliente envía una solicitud de mensaje a este servidor, este responderá y enviará el resultado al cliente. Algunas instrucciones de utilidad son: o rosservice list: devuelve la lista de los servicios activos. o rosservice info “nombre_servicio”: da información sobre el tipo del servicio y qué nodo lo publica. o rosservice call “nombre_servicio”: llama a un servicio desde la terminal.. •. Bags: son un formato para guardar los datos de mensajes durante la ejecución de un programa (como datos de sensores) y reproducirlos posteriormente permitiendo una depuración y análisis de resultados. Algunas instrucciones de utiliad son: o rosbag record -a: guarda a un archivo todos los topics en ejecución en el programa. o rosbag record “nombre_topics”: guarda a un archivo únicamente los topics indicados. o rosbag play “archivo.bag”: ejecuta los datos almacenados en un archivo bag.. •. Transformadas (/tf): es un topic de ROS siempre presente, que permite relacionar los distintos marcos de coordenadas del sistema en ejecución. Ha de tenerse en cuenta que un marco de coordenadas puede tener varios hijos pero un único padre.. •. Para visualizar el árbol de transformadas puede emplearse la herramienta rqt.. 40.

(41) Herramientas utilizadas. 2.3 Herramientas de visualización y simuladores 2.3.1 Gazebo Tras el diseño y desarrollo 3D del modelo del robot, la siguiente fase es su simulación. Esto permitirá al desarrollador ver cómo se comporta el robot en un entorno de simulación con un alto grado de fidelidad, similar al del mundo real. El entorno de simulación empleado es Gazebo, ya que tiene una relación nativa con ROS y permite una compleja simulación multirobot tanto en interiores como exteriores, así como los sensores de los robots (IMU, brújula, GPS, ultrasonidos, infrarojos, cámaras de visión...) y una variedad de objetos 3D, muchos de ellos disponibles en sus repositorios, sin necesidad de tener que crearlos.. FIGURA 2.11 Gazebo. 41.

(42) SLAM basado en láser para el robot aéreo ErleCopter. Gazebo puede ser instalado sin ROS, debiendo instalar el plugin ROS-Gazebo para la comunicación entre ambos, pues al ser un nodo para ROS, puede publicar y suscribirse a cualquier topic de ejecución. En cuanto a la versión utilizada será Gazebo 7.9, ya que es perfectamente compatible con ROS Indigo. Gazebo es un simulador que tendrá en cuenta aceleraciones, inercias, fuerzas gravitatorias... ErleCopter está actualmente implementado en Gazebo, lo que facilitará el desarrollo del proyecto al tener únicamente que descargarse en instalar el modelo el drone del repositorio.. 2.3.2 rqt rqt es un paquete específico de ROS, que será de utilidad para observar el árbol de transformadas y los gráficos de nodos y topics. Por ejemplo, tras ejecutar Gazebo, el gráfico de Nodos/Tópics facilitado mediante la actual herramienta es el siguiente:. FIGURA 2.12 rqt. 42.

(43) Herramientas utilizadas. Como se puede apreciar, deben distinguirse los nodos gazebo y mavros, ambos comunicados a través del tópic de transformadas /tf. Mavros es el puente oficial entre ROS y el protocolo MAVLink empleado por el autopiloto erlecopter. A su vez se distinguen diferentes topics lanzados por el modelo del erlecopter, así como los de los sensores ultrasonidos incorporados /sonar_front y /sonar_down.. 2.3.3 Rviz Rviz es un software que permite la visualización de diferentes datos de ejecución dentro de un sistema ROS (nodo roscore activo), permitiendo la representación de los tipos de datos más comunes como odometría, láser, sonar, mapas... Para ejecutarse basta con teclearse rosrun rviz rviz.. FIGURA 2.13 rviz. Al abrir la interfaz, se pueden incorporar distintas visualizaciones, algunas de las que se emplearán para el desarrollo de este proyecto son: •. “Map” permite observar el mapa que irá generando hector_mapping.. •. “Path” muestra la trayectoria que sigue el ErleCopter, será facilitada por hector_trajectory_server.. 43.

(44) SLAM basado en láser para el robot aéreo ErleCopter. •. “TF” añade los ejes de coordenadas de las diferentes transformadas a la visualización (map, base_link, sonar2_link…).. •. “Laser_scan” presenta el contorno de aquellos obstáculos detectados en el entorno mediante el láser.. •. “RobotModel” incluye el modelo del robot ErleCopter.. •. “Camera” incorpora la imagen de la cámara integrada a tiempo real.. Por otro lado, es importante configurar el “fixed frame” de las “Global Options” que aparecen a la izquierda. Este es el sistema de coordenadas que se toma como referencia para mostrar los datos. Por lo general, se suele tomar un punto estático como el world o el map.. 2.3.4 APM Planner APM Planner 2.0 es una aplicación de código abierto de estación terrestre para autopilotos basados en MAVlink, incluido el ErleCopter. Una vez conectado el autopiloto a APM Planner y a MavLink, esta herramienta proporcionará datos del estado del MAV así como del vuelo. A continuación, se ilustran algunos de ellos.. FIGURA 2.14 Datos de vuelo APM Planner. 44.

(45) Herramientas utilizadas. De igual modo, APM Planner permite la configuración del modo de vuelo, así como armar y desarmar el Erlecopter. En caso de utilizar GPS, posibilita la planificación de una misión mediante diferentes puntos intermedios a diferentes velocidades.. FIGURA 2.15 Armado y configuración de modo en APM Planner. Como puede apreciarse, esta herramienta tiene una mayor orientación hacia entornos en exterior, donde la señal GPS es buena. Puesto que la aplicación está dirigida a entornos de interior, no ha sido utilizada en profundidad, en cambio, ha posibilitado una primera puesta en marcha y primeros vuelos mediante diferentes modos de vuelo, que han permitido un control del Erlecopter muy satisfactorio.. 45.

(46) SLAM basado en láser para el robot aéreo ErleCopter. 3 Arquitectura general del sistema La finalidad de este capítulo es introducir y estructurar la arquitectura general del sistema, tanto software como hardware, detallando la implementación sobre la simulación, así como sobre la plataforma real.. 3.1 Arquitectura HW y SW en plataforma real El desarrollo de la arquitectura software, se basa en el uso de dos plataformas encargadas de la ejecución de diferentes procesos, distinguiendo: •. Unidad de vuelo: ErleCopter, dotado de Erle-Brain 2 basado en Linux con ROS instalado. Este permitirá la ejecución del Autopiloto, así como la lectura y envío de los datos obtenidos por los sensores a bordo del MAV y los comandos de radiocontrol.. •. Estación de tierra remota: PC con sistema operativo Linux, ROS instalado y capacidad de comunicar con el Erle-Copter vía wifi. El algoritmo SLAM será ejecutado en este, así como las tareas de control.. Esta arquitectura distribuida, se fundamenta en las limitaciones Hardware del controlador de la unidad de vuelo, puesto que lo óptimo sería una completa ejecución a bordo el MAV. De este modo se eilimiarían los tiempos de retardo en la transmisión de datos vía Wifi y el robot presentaría una mayor autonomía. El uso de ROS como herramienta de desarrollo, facilita en gran medida la ejecución a través de un sistema distribuido. A continuación, se ilustra un esquema general de la arquitectura implentada:. 46.

(47) Arquitectura general del sistema. FIGURA 3.1 Arquitectura general del sistema. Como se aprecia en el esquema mostrado, el Erle-Brain2 embebido en el Erle-Copter, es el encargado de realizar la lectura de los sensores. En este proyecto, se han empleado los sensores Láser Hokiyo y ultrasonidos, añadidos adicionalmente, y comunicándolos con el Erle-Brain2 mediante interfaz USB e I2C respectivamente. Así mismo, la imagen muestra otros posibles sensores a utilizar, ya integrados en el propio robot como altímetro, IMU o cámara entre otros. Por otro lado, la unidad remota mantiene una comunicación vía wifi para la transmisión de datos entre estaciones. Esta es la encargada de recibir estos datos, procesarlos y ejecutar el algoritmo SLAM: tanto el Scan Matcher de la información generada por el láser, como la respectiva fusión con los datos de los ultrasonidos mediante el Filtro Extendido de Kalman. De este modo, se obtiene una estimación robusta de la posición 6DoF del robot. A su vez, la posición estimada del coptero a la salida del EKF, posibilitará ejecutar en la estación de tierra un controlador PID de su posición, el cual permitirá definir unas consignas de radio-control mediante roll, pitch, yaw y throttle, que posibilitarán la navegación de la trayectoria deseada.. 47.

(48) SLAM basado en láser para el robot aéreo ErleCopter. Por último, estos comandos de radio-control son nuevamente enviados a la unidad de vuelo vía Wifi para ser ejecutados por el Autopiloto.. 3.2 Arquitectura HW y SW en plataforma simulada Previamente a una ejecución directa sobre el robot real, es deseable llevar a cabo una puesta en marcha sobre una simulación que se asemeje en lo mayor posible a la realidad para estudiar el comportamiento del robot. Esto ha sido posible mediante el uso de la herramienta Gazebo, un entorno de simulación con un alto grado de fidelidad, donde ErleRobotics dispone abiertamente del modelo del ErleCopter para obtener unos resultados similares a los del mundo real. En esta situación, al no disponer de unidad de vuelo física, es Gazebo el encargado de reproducir los datos generados por los ultrasonidos y el láser, para poder llevar a cabo la posterior ejecución de Hector mapping, el EKF, el PID, y el correspondiente envío a Gazebo de los comandos de radio-control para la simulación de la trayectoria.. Los posteriores capítulos, detallan la implentación de cada uno de los bloques principales mostrados. En primer lugar, se describe el algoritmo Hector mapping, así como la actuación sobre ciertos parámetros para la obtención de unos mejores resultados. En segundo lugar, se describe el Filtro de Kalman Extendido con los correspondientes modelos de predicción y corrección. Por último, se muestran los PIDs desarrollados, con orientación a la simulación y al robot real.. 48.

(49) Hector mapping. 4 SLAM: Hector mapping El problema del SLAM (Mapeo y Localizacion simultánea) ha sido un tema transcendental en investigación en el campo de la robótica móvil. Este problema trata si es posible ser ubicado un robot móvil en una posición desconocida dentro de un entorno desconocido para que el robot sea capaz de construir un mapa de ese entonro mientras determina de forma simultánea su ubicación dentro de este. El concepto del problema SLAM surgió en la Conferencia de Automatizacion y Robótica IEEE de 1986 celebrada en San Francisco. En este instante los métodos probabilísticos comenzaron a abrirse terreno en el campo de la robótica y la inteligencia artificial, mostrándose tras años de investigación cómo a medida que un robot móvil se mueve a través de un entorno desconocido, tomando observaciones relativas de puntos de referencia, estas estimaciones están necesariamente correlacionadas entre sí debido al error común en la ubicación estimada del robot. La solución del problema SLAM ha sido uno de los grandes éxitos de la robótica. Este se ha formulado y resuelto en diferenes marcos teóricos y se ha implementado en diferentes modelos de robots tanto indoor como outdoor en entornos terrestres, aéreos y submarinos.. 4.1 Hector mapping El equipo Hector [5] (Heterogeneous Cooperating Team of Robots) de la Universidad Técnica de Darmstadt desarrolló este paquete en la competición RoboCup RPL Final 2012, orientado a la implantación en robots que puedan ser introducidos en entornos catastróficos. Por ello debe considerarse que operan en entornos desconocidos para que presenten mayor robustez contra los cambios, lo que significa que el problema de SLAM debe ser resuelto para generar un mapeado útil para la navegación de los primeros rescatadores u otros robots. Para esta tarea proporcionaron hector slam, el cual consta de hector_mapping, hector_map_server, hector_geotiff y hector_trajectory_server. Puesto que la odometría no presenta alto grado de fiabilidad en escenarios catastróficos debido a la irregularidad 49.

(50) SLAM basado en láser para el robot aéreo ErleCopter. de la superficie, el sistema esta diseñado para no requerir de datos de odometría. En su lugar, dependen de la rápida adquisición de datos LIDAR combiado con un sistema de estimación de altitud y una unidad opcional de pitch/roll para estabilizar el escaneo del láser. Este algoritmo open-source construye un mapa de cuadrícula 2D del entorno que rodea al robot, basado en el escaneo del láser y observable en rviz, donde las superficies vacías vienen dadas en color gris, y los objetos detectados en el entorno se muestran en negro:. FIGURA 4.1 Mapa de cuadrícula 2D de hector_mapping y láser (Rviz). 4.1.1 Acceso al mapa Para proporcionar una posición 3D, permitiendo movimientos en 6DoF, es decir, posibilitando movivientos en los 3 ejes, combinando traslaciones y rotaciones, Hector emplea un sistema de representación de coordenadas del siguiente tipo: 𝑥⃗ = (𝛺⃗⃗𝑇 𝑝⃗𝑇 𝑣⃗ 𝑇 )𝑇 Expresado en ángulos de Euler: 𝛺⃗⃗ = (𝜙, 𝜎, 𝜓)𝑇.

(51) Hector mapping. La posición: 𝑝⃗ = (𝑝𝑥 , 𝑝𝑦 , 𝑝𝑧 )𝑇 La velocidad 𝑣⃗ = (𝑣𝑥 , 𝑣𝑦 , 𝑣𝑧 )𝑇 Para fusionar los datos de un nuevo escaneo con un mapa ya existente, se aproximan las muestreas en varias resoluciones y comienza a fusionarlos con la resolución más baja. A diferencia de otros procesadores de imágenes comunes, hector almacena diferentes mapas en memoria (con la escala adecuada). Los mapas se alinean a medida que se van actualizando, evitando de este modo que el proceso se atasque en un mínimo local. Para realizar dicha aproximación, las coordenadas del mapa 𝑃⃗⃗𝑚 , el mapa de ocupación 𝑀(𝑃⃗⃗𝑚 ) (celdas ocupadas), y el gradiente del mapa 𝛻𝑀(𝑃⃗⃗𝑚 ) 𝛻𝑀(𝑃⃗⃗𝑚 ) = (. 𝜕𝑀 𝜕𝑀 (𝑃⃗⃗𝑚 ), (𝑃⃗⃗ )) 𝜕𝑥 𝜕𝑦 𝑚. Son linealmente interpolados a los cuatro enteros más próximos de las coordendas en el mapa 𝑃⃗⃗00 , 𝑃⃗⃗01 , 𝑃⃗⃗10 , 𝑃⃗⃗11 , los cuales representan los puntos medios de las celdas de la cuadrícula del mapa: 𝑀(𝑃⃗⃗𝑚 ) ≈. 𝑦 − 𝑦0 𝑥 − 𝑥0 𝑥1 − 𝑥 ( 𝑀(𝑃⃗⃗11 ) + 𝑀(𝑃⃗⃗01 )) 𝑦1 − 𝑦0 𝑥1 − 𝑥0 𝑥1 − 𝑥0 +. 𝑦1 − 𝑦 𝑥 − 𝑥0 𝑥1 − 𝑥 ( 𝑀(𝑃⃗⃗10 ) + 𝑀(𝑃⃗⃗00 )) 𝑦1 − 𝑦0 𝑥1 − 𝑥0 𝑥1 − 𝑥0. Y con las derivadas en el espacio: 𝜕𝑀 𝑦 − 𝑦0 𝑦1 − 𝑦 (𝑃⃗⃗𝑚 ) ≈ (𝑀(𝑃⃗⃗11 ) − 𝑀(𝑃⃗⃗01 )) + (𝑀(𝑃⃗⃗10 ) − 𝑀(𝑃⃗⃗00 )) 𝜕𝑥 𝑦1 − 𝑦0 𝑦1 − 𝑦0 𝜕𝑀 𝑥 − 𝑥0 𝑥1 − 𝑥 (𝑃⃗⃗𝑚 ) ≈ (𝑀(𝑃⃗⃗11 ) − 𝑀(𝑃⃗⃗10 )) + (𝑀(𝑃⃗⃗01 ) − 𝑀(𝑃⃗⃗00 )) 𝜕𝑦 𝑥1 − 𝑥0 𝑥1 − 𝑥0 A continuación, se ilistra un mapa donde aparecen los valores enteros de las coordenadas en el mapa 𝑃⃗⃗00..11 más próximas al punto 𝑃⃗⃗𝑚 , el cual es interpolado, así como la cuadrícula ocupada y sus derivadas en el espacio: 51.

(52) SLAM basado en láser para el robot aéreo ErleCopter. FIGURA 4.2 Representación del mapa (cuadrícula ocupada). FIGURA 4.3 Predicción de la posición (derivadas en el espacio). 4.1.2 Scan matching El concepto de scan matching consiste en alinear el scan actual leído mediante el LIDAR con un entorno de referencia ya escaneado previamente, es decir, un mapa. Para ello, el enfoque de Hector realiza correspondencia de datos (alineación de mediciones tomadas en el instante t con mediciones tomadas en instantes previos) aplicando el algoritmo de Gauss-Newton, donde se busca la transformación rígida mediante la siguiente expresión:.

(53) Hector mapping. 𝑛. 𝜉⃗∗ = 𝑎𝑟𝑔𝑚𝑖𝑛𝜉 ∑[1 − 𝑀(𝑆⃗𝑖 (𝜉⃗))]2 𝑖=1. Esta ecuación permite encontrar la mejor alineación de mediciones del haz del láser con el mapa actual, donde 𝑆⃗𝑖 (𝜉⃗) representa las coordenadas del mundo de un punto que está siendo escaneado a partir de la posición actual del robot. Y 𝜉⃗ la transformada rígida de la nube de puntos proyectada en el mapa: 𝜉⃗ = (𝑝𝑥 , 𝑝𝑦 , 𝜓)𝑇 En cuanto a las coordenadas del robot en el mapa, se tiene que: 𝑝𝑥 cos(𝜓) − sin(𝜓) 𝑠𝑖,𝑥 𝑆⃗𝑖 (𝜉⃗) = ( ) (𝑠 ) + (𝑝 ) 𝑖,𝑦 𝑦 sin(𝜓) cos(𝜓) La función 𝑀(𝑆⃗𝑖 (𝜉⃗)) devuelve las coordenadas obtenidas del robot en el mapa indicadas mediante 𝑆⃗𝑖 (𝜉⃗), con una aproximación inicial de 𝜉⃗ para estimar ∆𝜉⃗, de modo que logra minimizarse el error. Por tanto: 𝑛. ∑[1 − 𝑀(𝑆⃗𝑖 (𝜉⃗ + ∆𝜉⃗))]2 → 0 𝑖=1. Si se aplica la serie de Taylor, esta ecuación puede expresarse como: 𝑛. 𝜕𝑆⃗𝑖 (𝜉⃗) ∑ [1 − 𝑀 (𝑆⃗𝑖 (𝜉⃗)) − 𝛻𝑀(𝑆⃗𝑖 (𝜉⃗)) ∆𝜉⃗] ⃗ 𝜕𝜉. 2. → 0. 𝑖=1. Calculando la derivada parción respecto a ∆𝜉⃗, se tiene que: 𝑇. 𝑛. 𝜕𝑆⃗𝑖 (𝜉⃗) 𝜕𝑆⃗𝑖 (𝜉⃗) ] · [1 − 𝑀 (𝑆⃗𝑖 (𝜉⃗)) − 𝛻𝑀 (𝑆⃗𝑖 (𝜉⃗)) 2 · ∑ [𝛻𝑀 (𝑆⃗𝑖 (𝜉⃗)) ∆𝜉⃗] = 0 𝜕𝜉⃗ 𝜕𝜉⃗ 𝑖=1. Por último, el error ∆𝜉⃗ es minimizado aplicando el algoritmo de Gauss-Newton 𝑇. 𝑛. 𝜕𝑆⃗𝑖 (𝜉⃗) ] · [1 − 𝑀 (𝑆⃗𝑖 (𝜉⃗))] ∆𝜉⃗ = 𝐻 −1 ∑ [𝛻𝑀 (𝑆⃗𝑖 (𝜉⃗)) 𝜕𝜉⃗ 𝑖=1. Con la matriz Hessiana: 53.

(54) SLAM basado en láser para el robot aéreo ErleCopter. 𝑇. 𝜕𝑆⃗𝑖 (𝜉⃗) 𝜕𝑆⃗𝑖 (𝜉⃗) ] · [𝛻𝑀 (𝑆⃗𝑖 (𝜉⃗)) ] 𝐻 = [𝛻𝑀 (𝑆⃗𝑖 (𝜉⃗)) 𝜕𝜉⃗ 𝜕𝜉⃗ Aplicando la ecuación del gradiente del mapa 𝛻𝑀(𝑃⃗⃗𝑚 ) y la de las coordenadas del robot en el mapa 𝑆⃗𝑖 (𝜉⃗), se obtiene que: 1 𝜕𝑆⃗𝑖 (𝜉⃗) =( 0 𝜕𝜉⃗. 0 1. − sin(𝜓) · 𝑠𝑖,𝑥 − cos(𝜓) · 𝑠𝑖,𝑦 ) cos(𝜓) · 𝑠𝑖,𝑥 − 𝑠𝑖𝑛(𝜓) · 𝑠𝑖,𝑦. Empleando las expresiones de 𝛻𝑀(𝑃⃗⃗𝑚 ) y. ⃗⃗ ) 𝜕𝑆⃗𝑖 (𝜉 ⃗⃗ 𝜕𝜉. , la aproximación de Newton es evaluada. paso a paso hacia un error ∆𝜉⃗ mínimo. A pesar de ello, debe saberse que no puede garantizarse el mínimo debido a las a aproximaciones lineales no uniformes del gradiente del mapa 𝛻𝑀(𝑃⃗⃗𝑚 ). En cuanto a la covarianza, es aproximada mediante: 𝑅 = 𝑉𝑎𝑟𝜉 = 𝜎 2 · 𝐻−1 Siendo 𝜎 un factor de escalado, el cual es dependiente de las propiedades del sensor láser.. 4.2 Implementación de Hector mapping Antes de comenzar la implementación de Hector mapping, es recomendable ajustar las características del láser simulado en Gazebo, con las del láser Hokuyo que se dispone. Por ello, se ha realizado la siguiente configuración en /erlecopter.xacro ubicado en ~/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plu gin/urdf: xacro:lidar_sensor: min_range="0.6" max_range="4.095" field_of_view_horizontal="${240*M_PI/180}" ray_count_horizontal="666" // 240º/0.26º=666 rayos. La imagen indicada a continuación muestra los ejes de coordenadas de mayor importancia en una vista 3D simplificada:.

(55) Hector mapping. FIGURA 4.4 Transformaciones de Hector Mapping. Se parte de una relación entre las coordenadas map y world idéntica. Por otro lado, el marco de coordenadas base_link, está solidario al robot, agregando información sobre la posición 2D del robot (posición y orientación), así como el ángulo yaw con respecto a las coordenadas del mapa. Por último, la transformación de base_link a sonar2_link viene proporcionada por una transformada estática que añade el offset del láser Hokuyo. Por otro lado, los sensores lanzados desde erlecopter.xacro y erlecopter_base.xacro (~/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_pl ugin/urdf) son los siguientes:. tf. topic. LASER. sonar2_link. /scan. IMU. erlecopter/imu_link. /erlecopter/imu. SONAR_HEIGH. sonar_link. /sonar_down. SONAR_FRONT. sonar2_link. / sonar_front. CAMERA_BOTTOM. erlecopter_bottomcam. / erlecopter/bottom/image_raw. CAMERA_FRONTAL. erlecopter_frontcam. / erlecopter/front/image_raw. TABLA 4.1 Sensores, TFs y topics. 55.

(56) SLAM basado en láser para el robot aéreo ErleCopter. Por lo que ha sido necesario generarse un paquete al que se le ha llamado “hector_mapping” para llevar a cabo el lincado de los sensores del erlecopter con la transformada base_link mediante transformadas estáticas. Para ello se ha desarrollado el fichero erle_mapping.launch, el cual implementa dichas transformadas y en último lugar lanza la simulación de Gazebo: <launch> <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 1 world map 100" /> <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0 0 0 0 1 base_link sonar2_link 100" /> <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="0 0 0 0 0 0 1 base_link erlecopter/imu_link 100" /> <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_down" args="0 0 0 0 0 0 1 base_link sonar_link 100" /> <node pkg="tf" type="static_transform_publisher" name="base_link_to_bottom_cam" args="0 0 0 0 0 0 1 base_link erlecopter_bottomcam 100" /> <node pkg="tf" type="static_transform_publisher" name="base_link_to_front_cam" args="0 0 0 0 0 0 1 base_link erlecopter_frontcam 100" /> <include file="/home/nicolas/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ ardupilot_sitl_gazebo_plugin/launch/erlecopter_spawn.launch"> </include> </launch>. De este modo, una vez lanzado Gazebo, si se ejecuta hector_mapping.launch, se observa cómo se añaden al árbol las transformadas estáticas generadas:. FIGURA 4.5 TFs estáticas de los sensores (rqt).

(57) Hector mapping. En cualquier caso, no existe relación alguna entre el marco de coordenadas map y base_link, pues este debe establecerlo automáticamente la ejecución del fichero hector_mapping. A pesar de ello, si se lanza hector mapping por defecto, se genera un fallo que indica un error en la transformación entre los marcos de coordenadas odom y base_link. Al observar el árbol de transformadas, se aprecia cómo hector mapping ha generado dos transformadas nuevas denominadas scanmatcher_frame y odom que cuelgan directamente de map. En cualquier caso, sigue sin establecerse enlace alguno con la tf solidaria al ErleCopter (base_link). Esto se ilustra a continuación:. FIGURA 4.6 TFs tras ejecutar hector_mapping por defecto (rqt). Por ello, para solventarlo, ha sido necesario modificar el archivo mapping_default.launch ubicado en ~/simulation/ros_catkin_ws/src/hector_slam/hector_mapping/launch: <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/> <arg name="base_frame" default="base_footprint"/> <arg name="odom_frame" default="nav"/> <arg name="pub_map_odom_transform" default="true"/>. Por las siguientes lineas:. <arg name="tf_map_scanmatch_transform_frame_name" default="base_link"/> <arg name="base_frame" default="base_link"/> <arg name="odom_frame" default="base_link"/> <arg name="pub_map_odom_transform" default="false"/>. 57.

(58) SLAM basado en láser para el robot aéreo ErleCopter. Mediante estas modificaciones, tras lanzar hector_mapping se logra establecer la relación de transformación entre el marco de coordenadas map y base_link:. FIGURA 4.7 TFs tras configurar hector_mapping (Rviz). A su vez, el gráfico de nodos/topics activos obtenido hasta el momento resulta del siguiente modo:.

(59) Hector mapping. FIGURA 4.8 Nodos/topics activos tras configurar hector_mapping. Como se puede ver, hector_mapping lee los datos del /scan y a su vez publica el mapa /map. Haciendo uso de la herramienta RViz, se observa cómo se genera el mapeado con respecto a aquellas paredes u obstáculos que es capaz de detectar el radio de alcance del láser Hokuyo.. 59.

(60) SLAM basado en láser para el robot aéreo ErleCopter. FIGURA 4.9 Visualización de mapa generado por hector_mapping (Rviz). 4.3 Implementación de Hector mapping en robot real En este punto, se detalla cómo establecer una conexión con el ErleCopter físico, así como la inicialización de cada uno de los sensores a utilizar. Por otro lado, se lleva a cabo el registro de los topics generados en un fichero.bag de modo que puedan ser reproducidos a posteriori para establecer resultados y conclusiones. Erlecopter: 1. Conectar desde PC remoto con SSH $ ssh [email protected]. 2. Iniciar Láser Hokuyo: $ cd /dev $ sudo chmod a+rw ttyACM0 # Por defecto los sistemas GNU/linux no habilitan el modo escritura en el puerto, por lo que se deben habilitar manualmente. No es recomendable trabajar como root, por ello, se dan permisos de lectura y escritura al puerto al que está conectado el láser (ttyACM0). $ cd guillermo_nicolas_ws $ source devel/setup.bsah $ rosrun hokuyo_node hokuyo_node & # & al final de rosrun para permite mantener lanzado el nodo tras pulsar la tecla enter en la terminal..