UNIVERSIDAD COMPLUTENSE DE MADRID FACULTAD DE CIENCIAS FÍSICAS Departamento de Arquitectura de Computadores y Automática LOCALIZACIÓN Y SEGUIMIENTO DE TRAYECTORIAS CON ROBOTS CAMINANTES EN ENTORNOS NATURALES MEMORIA PARA OPTAR AL GRADO DE DOCTOR PRESENTADA POR José Antonio Cobano Suárez Bajo la dirección de los Doctores Pablo González de Santos y Joaquín Estremera Rodrigo Madrid, 2008 • ISBN: 978-84-669-3154-0 ©José Antonio Cobano Suárez, 2007 UNIVERSIDAD COMPLUTENSE DE MADRID Departamento de Arquitectura de Computadores y Automática TESIS DOCTORAL LOCALIZACIÓN Y SEGUIMIENTO DE TRAYECTORIAS CON ROBOTS CAMINANTES EN ENTORNOS NATURALES José Antonio Cobano Suárez 2007 UNIVERSIDAD COMPLUTENSE DE MADRID Departamento de Arquitectura de Computadores y Automática TESIS DOCTORAL LOCALIZACIÓN Y SEGUIMIENTO DE TRAYECTORIAS CON ROBOTS CAMINANTES EN ENTORNOS NATURALES Autor: José Antonio Cobano Suárez Directores: Dr. Pablo González de Santos Dr. Joaqúın Estremera Rodrigo 2007 A mis padres A Cristina Agradecimientos Esta sección quiero comenzarla expresando mi total agradecimiento al Dr. D. Pablo González de Santos y al Dr. D. Joaqúın Estremera Rodrigo. La ayuda de ambos ha sido fundamental para llevar a cabo esta tesis doctoral. Al Dr. D. Pablo González de Santos quiero agradecer su dedicación, apoyo, colaboración, comprensión y, sobre todo, su sobresaliente dirección. Al Dr. D. Joaqúın Estremera quiero agradecer la gran ayuda prestada, sus consejos y su disponibilidad en todo momento. A ellos les debo mucho de lo que he aprendido en estos años. Agradezo al Dr. D. Salvador Ros, director del Instituto de Automática Industrial (IAI-CSIC), todos los medios prestados para la realización de esta tesis doctoral. Del mismo modo, agradezco al Dr. D. Manuel Armada, vice-director del IAI-CSIC, su apoyo por las facilidades ofrecidas durante los años que ha sido Jefe del Departamento de Control Automático. También quiero extender este agradecimiento a todo el personal del IAI-CSIC por toda la ayuda recibida d́ıa a d́ıa. Entre todo el personal destaco la ayuda recibida del Dr. D. Eugenio Naranjo quien me asesoró en los secretos del GPS y al personal del taller mecánico: Román, Juan Carlos, Paco y Luis. Por otro lado, quiero agradecer al Dr. D. Jesús Manuel de la Cruz, tutor de esta tesis doctoral y del Trabajo Tutelado realizado para obtener el Diploma de Estudios Avanzados (DEA), por su ayuda y consejos durante la realización de dichos trabajos. Este agradecimiento deseo extenderlo al Departamento de Arquitectura de Computadores y Autómatica por ofrecerme la oportunidad de completar todo el programa de doctorado y la defensa de esta tesis. Hay compañeros a los que quiero agradecer especialmente: Dra. Elena Garćıa por sus claras explicaciones, Dr. Héctor Montes por su gran ayuda y consejos, Manuel Prieto por su bondadosa forma de compartir sus extensos conocimientos, Roberto Ponticelli por su ayuda y contagiosa alegŕıa, Jesús Reviejo por su ayuda vii y trabajo realizado en el robot SILO6, Carlota Salinas por el buen ambiente de trabajo que me brindó y su gran ayuda. También quiero agradecer con mucho cariño a los compañeros del departamento durante estos años: Tomás, Octavio, Roemi, Samir, Javier, Adriana, Cristian, Mauricio, Pedro, Jorge, Álvaro Lou, Álvaro Ramos, Dr. José de No, Dr. Teodor Akinfiev y Dr. Rony Caballero. A todos, gracias por vuestra amistad, ayuda y trato durante estos años. Al Dr. Andrew Howard y al Dr. Wolfram Burgard quiero agradecerles la oportunidad y ayuda que me ofrecieron en las estancias realizadas en la University Southern of California (Los Angeles, E.E.U.U) y en la Albert-Ludwigs-Universität Freigurg (Friburgo, Alemania), respectivamente. No voy a olvidarme de Cristina a quien tengo que agradecerle el amor, cariño, comprensión, apoyo y la manera de afrontar la distancia en este periodo de tiempo. Estos agradecimientos no pueden terminar sin un emotivo agradecimiento a mis padres, Antonio y Maŕıa del Carmen, por todo lo que me han dado y me están dando en esta vida. También por su entrega para que haya llegado hasta aqúı y por su constante amor. Al mismo tiempo agradecer el apoyo y cariño de mis hermanas Maŕıa del Carmen y Desirée. A mis “hermanos” gracias por cada segundo que me regalaron. Este trabajo ha sido financiado por una beca predoctoral de Formación de Personal Investigador (FPI) del Ministerio de Educación y Ciencia asociada al proyecto DPI2001-1595 y un contrato con cargo al proyecto DPI2004-05824 desarrollado en el Departamento de Control Automático del IAI-CSIC. Resumen Esta tesis doctoral se fundamenta en el diseño y experimentación de algoritmos para la detección y localización de minas antipersonas con robots caminantes. Esta tarea debe realizarse sobre terrenos naturales (suelos irregulares, con diferente dureza y en presencia de obstáculos) y en los que se pueden encontrar zonas prohibidas donde los pies no deben apoyarse (alarmas o minas antipersonas potenciales previamente detectadas por un sistema ajeno a este trabajo). La consecución de este complejo objetivo aconseja dividirlo en diferentes sub-objtetivos que se estudian independientemente: (a) modos de caminar: planificación de los movimientos de las patas del robot para que se produzca el avance en la dirección deseada manteniendo la estabilidad y adaptación al terreno, y teniendo en cuenta la existencia de zonas prohibidas; (b) localización: cálculo de la posición del robot en el entorno de trabajo durante toda la tarea; y por último, (c) seguimiento de trayectorias: definición de la trayectoria del robot para garantizar la exploración completa del terreno y generación de algoritmos de control para corregir los desv́ıos de la trayectoria. La solución independiente de estos tres sub-objetivos constituye el cuerpo principal de esta tesis doctoral. En primer lugar, se ha definido un modo de caminar libre de deriva y dos modos giratorios para robots hexápodos que desplazan el robot evitando que los pies pisen las zonas prohibidas sobre el terreno. En segundo lugar, se ha desarollado un sistema de localización basado en diferentes métodos (odometŕıa, dead-reckoning, DGPS y un filtro de Kalman extendido) que permiten conocer en todo instante la posición y orientación del robot con la precisión requerida para esta tarea, estimada en ±0.02 m. Por último, se ha generado un algoritmo de control de seguimiento de trayectorias para desplazar el robot a lo largo de una trayectoria predefinida y, poder aśı, realizar una exploración completa del terreno. Los algoritmos presentados se han validado experimentalmente utilizando el ix robot hexápodo SILO6. Índice General . Índice de figuras XVII . Índice de tablas XXV 1. Introducción 1 1.1. Robots caminantes . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2. Modos de caminar . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.3. Seguimiento de trayectorias . . . . . . . . . . . . . . . . . . . . . . 7 1.4. Localización de robots . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.5. Objetivos de la tesis . . . . . . . . . . . . . . . . . . . . . . . . . . 11 1.5.1. Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . 11 1.5.2. Objetivos globales . . . . . . . . . . . . . . . . . . . . . . . 11 1.6. Organización de la memoria . . . . . . . . . . . . . . . . . . . . . . 12 2. Modos de caminar 15 2.1. Generalidades . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 2.2. Modos ondulatorios . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.3. Modos de deriva . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.4. Modos discontinuos . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.5. Modos libres . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 2.6. Modos giratorios . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3. Generación de modos de caminar libres para robots hexápodos 29 3.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 3.2. Planteamiento general del modo de caminar . . . . . . . . . . . . . 30 3.3. Definiciones de estabilidad . . . . . . . . . . . . . . . . . . . . . . . 33 3.4. Modelo del terreno . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 3.5. Definición del volumen de trabajo . . . . . . . . . . . . . . . . . . . 38 XII ÍNDICE GENERAL 3.6. Definición de trayectorias de transferencia de las patas . . . . . . . 38 3.6.1. Definición de la trayectoria de un pie en transferencia . . . 39 3.6.2. Definición de la trayectoria de un tŕıpode en transferencia . 41 3.6.3. Definición de magnitudes . . . . . . . . . . . . . . . . . . . 41 3.7. Planificador de levantamiento de patas . . . . . . . . . . . . . . . . 44 3.8. Movimiento del cuerpo . . . . . . . . . . . . . . . . . . . . . . . . . 45 3.9. Búsqueda de apoyos . . . . . . . . . . . . . . . . . . . . . . . . . . 46 3.9.1. Condiciones para los puntos de apoyo . . . . . . . . . . . . 47 3.9.2. Criterios de optimización de los puntos de apoyo . . . . . . 49 3.9.3. Algoritmo de búsqueda de los puntos de apoyo . . . . . . . 50 3.10. Modos giratorios . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 3.10.1. Modo circular . . . . . . . . . . . . . . . . . . . . . . . . . . 51 3.10.2. Modo rotatorio . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.11. Valoración del algoritmo mediante simulación . . . . . . . . . . . . 55 3.12. Valoración del algoritmo experimentalmente . . . . . . . . . . . . . 65 4. Técnicas de localización en exteriores 69 4.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69 4.2. El problema de la localización . . . . . . . . . . . . . . . . . . . . . 69 4.2.1. Caracteŕısticas de la localización . . . . . . . . . . . . . . . 71 4.2.1.1. Entorno de la tarea . . . . . . . . . . . . . . . . . 71 4.2.1.2. Conocimiento sobre el entorno . . . . . . . . . . . 72 4.2.1.3. Tipo de tarea . . . . . . . . . . . . . . . . . . . . . 72 4.2.1.4. Sistema sensorial . . . . . . . . . . . . . . . . . . . 73 4.2.2. Casos t́ıpicos de la localización . . . . . . . . . . . . . . . . 73 4.2.3. Información disponible por el robot . . . . . . . . . . . . . . 74 4.2.4. Tipos de localización . . . . . . . . . . . . . . . . . . . . . . 74 4.2.4.1. Localización relativa . . . . . . . . . . . . . . . . . 75 4.2.4.2. Localización absoluta . . . . . . . . . . . . . . . . 75 4.3. Localización en exteriores . . . . . . . . . . . . . . . . . . . . . . . 75 4.3.1. Medidas internas . . . . . . . . . . . . . . . . . . . . . . . . 75 4.3.1.1. Odometŕıa . . . . . . . . . . . . . . . . . . . . . . 76 4.3.1.2. Sistemas de navegación inercial: dead-reckoning . 77 4.3.2. Medidas externas . . . . . . . . . . . . . . . . . . . . . . . . 78 4.3.2.1. Estaciones de transmisión . . . . . . . . . . . . . . 78 4.3.2.2. Estimación mediante balizas . . . . . . . . . . . . 79 4.4. Fusión Sensorial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 ÍNDICE GENERAL XIII 4.4.1. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . 81 4.4.2. Método de Montecarlo . . . . . . . . . . . . . . . . . . . . . 82 4.4.3. Filtro de part́ıculas . . . . . . . . . . . . . . . . . . . . . . . 82 4.5. Localización cooperativa o multi-robot . . . . . . . . . . . . . . . . 83 5. Fusión sensorial 85 5.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 5.2. Localización Bayesiana . . . . . . . . . . . . . . . . . . . . . . . . . 85 5.2.1. Modelo de observación . . . . . . . . . . . . . . . . . . . . . 87 5.2.2. Modelo de movimiento . . . . . . . . . . . . . . . . . . . . . 88 5.3. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 5.3.1. Sistema dinámico lineal . . . . . . . . . . . . . . . . . . . . 89 5.3.2. Sistema dinámico no lineal . . . . . . . . . . . . . . . . . . 91 5.4. Filtro de part́ıculas . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 6. Algoritmo de localización para el Robot SILO6 97 6.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 6.2. Fase de estimación . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 6.2.1. Dead-reckoning con encoders y brújula electromagnética. . 100 6.2.2. Dead-reckoning con encoders, brújula electromagnética e inclinómetro. . . . . . . . . . . . . . . . . . . . . . . . . . . 103 6.3. Fase de actualización . . . . . . . . . . . . . . . . . . . . . . . . . . 111 6.4. Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores . . . . . . . . . . . . . . . . . . 113 6.4.1. Estudio del Sistema Sensorial . . . . . . . . . . . . . . . . . 113 6.4.1.1. Sistema de Posicionamiento Global Diferencial . . 113 6.4.1.2. Odometŕıa . . . . . . . . . . . . . . . . . . . . . . 122 6.4.1.3. Brújula electromagnética . . . . . . . . . . . . . . 127 6.4.1.4. Dead-reckoning . . . . . . . . . . . . . . . . . . . . 132 6.4.2. Fusión Sensorial: EKF . . . . . . . . . . . . . . . . . . . . . 137 6.4.2.1. EKF con encoders, brújula electromagnética y DGPS . . . . . . . . . . . . . . . . . . . . . . . . . 139 6.4.2.2. EKF con encoders, brújula electromagnética, inclinómetro y DGPS . . . . . . . . . . . . . . . . 143 7. Seguimiento de trayectorias con máquinas caminantes 149 7.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149 7.2. Definición de la trayectoria deseada . . . . . . . . . . . . . . . . . . 151 XIV ÍNDICE GENERAL 7.3. Control de seguimiento de trayectoria . . . . . . . . . . . . . . . . 153 7.3.1. Control azimutal del robot . . . . . . . . . . . . . . . . . . 155 7.3.2. Control de posición horizontal del robot . . . . . . . . . . . 155 7.3.3. Control de velocidad de avance del robot . . . . . . . . . . 156 7.3.4. Control de cambio de trayectoria y selección de modo de caminar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156 7.4. Valoración del algoritmo mediante simulación . . . . . . . . . . . . 158 7.5. Valoración del algoritmo experimentalmente . . . . . . . . . . . . . 161 7.5.1. EXP-3: Seguimiento de una trayectoria rectiĺınea . . . . . . 162 7.5.2. EXP-4: Trayectoria de exploración completa . . . . . . . . . 166 . Principales aportaciones 179 A. Glosario de términos 187 B. Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA 191 B.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191 B.2. Descripción del sistema y requisitos básicos . . . . . . . . . . . . . 195 B.3. Detección y localización de minas . . . . . . . . . . . . . . . . . . . 199 B.3.1. Cabeza sensora. . . . . . . . . . . . . . . . . . . . . . . . . 199 B.3.2. Localizador . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 B.4. Manipulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202 B.5. Configuración del robot caminante . . . . . . . . . . . . . . . . . . 204 B.5.1. Estructura del cuerpo . . . . . . . . . . . . . . . . . . . . . 204 B.5.2. Estructura de la pata . . . . . . . . . . . . . . . . . . . . . 205 B.6. Sistema de control . . . . . . . . . . . . . . . . . . . . . . . . . . . 208 B.6.1. Estación de operación . . . . . . . . . . . . . . . . . . . . . 208 B.6.1.1. Interfaz operario/robot . . . . . . . . . . . . . . . 209 B.6.1.2. Gestor de la base de datos . . . . . . . . . . . . . 209 B.6.1.3. Comunicación con el robot caminante . . . . . . . 209 B.6.2. Controlador del robot . . . . . . . . . . . . . . . . . . . . . 210 B.6.2.1. Arquitectura Hardware . . . . . . . . . . . . . . . 210 B.6.2.2. Arquitectura Software . . . . . . . . . . . . . . . 210 C. El Sistema de Posicionamiento Global 215 C.1. Configuración del sistema . . . . . . . . . . . . . . . . . . . . . . . 215 C.2. Funcionamiento del sistema . . . . . . . . . . . . . . . . . . . . . . 216 ÍNDICE GENERAL XV C.3. Errores del sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . 219 C.4. El Sistema de Posicionamiento Global Diferencial . . . . . . . . . . 220 C.5. Elementos del Sistema Diferencial . . . . . . . . . . . . . . . . . . . 220 D. Geometŕıa del modo de caminar 223 XVI ÍNDICE GENERAL Índice de Figuras 1.1. General Electric Walking Truck, (E.E.U.U.). . . . . . . . . . . . . . 2 1.2. Dante II (Universidad Carnegie-Mellon, E.E.U.U.). . . . . . . . . . 3 1.3. The Adaptive Suspension Vehicle - ASV (Universidad del Estado de Ohio, E.E.U.U). . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.4. ROWER (Instituto de Automática Industrial – CSIC, España). . . 6 1.5. Timberjack (Plustech-Oy, Finlandia). . . . . . . . . . . . . . . . . . 6 1.6. Roboclimber (Instituto de Automática Industrial – CSIC, España). 8 1.7. Seguimiento de trayectorias con (a) modos de deriva y (b) modos giratorios. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.1. (a) El robot caminante AMBLER (Universidad Carnegie-Mellon, E.E.U.U.) y (b) descripción del modo circular. . . . . . . . . . . . 18 2.2. Definiciones geométricas. . . . . . . . . . . . . . . . . . . . . . . . . 19 2.3. Margen de estabilidad de un poĺıgono de apoyo. . . . . . . . . . . . 21 2.4. Margen de estabilidad longitudinal de un poĺıgono de apoyo. . . . 21 2.5. Diagramas de tiempos: (a) Numeración de patas en un robot, (b) Diagrama de tiempos de un modo ondulatorio β = 5/6. (c) Diagrama de tiempos de un modo ondulatorio β = 1/2 (tŕıpodes alternados). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.6. Modos discontinuos. . . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.7. Modos circulares. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 2.8. Modos rotatorios. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.1. Definiciones básicas. . . . . . . . . . . . . . . . . . . . . . . . . . . 34 3.2. Definiciones relacionadas con la estabilidad. . . . . . . . . . . . . . 35 3.3. Modelo del terreno y trayectoria de transferencia vista desde un sistema de referencia externo: (a) en 3-D, (b) vista lateral. . . . . . 37 3.4. Recorrido cinemático y volumen de trabajo de la pata. . . . . . . . 38 XVIII ÍNDICE DE FIGURAS 3.5. Definición de la trayectoria de transferencia en el sistema de referencia del robot. . . . . . . . . . . . . . . . . . . . . . . . . . . 40 3.6. Trayectoria de transferencia en el sistema de referencia del robot: (a) vista lateral, (b) vista superior. . . . . . . . . . . . . . . . . . . 42 3.7. Candidatos a puntos de apoyo empleados en la planificación de apoyos del robot SILO6. . . . . . . . . . . . . . . . . . . . . . . . . 50 3.8. Algoritmo de búsqueda de apoyos. . . . . . . . . . . . . . . . . . . 52 3.9. Evolución del modo de caminar a lo largo de medio ciclo de locomoción t́ıpico. . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 3.10. (a) Diagrama de tiempos: pata en apoyo (ĺınea continua), pata en transferencia (ĺınea discontinua); (b) Margen de estabilidad absoluto. 57 3.11. Modo de caminar al aumentar la constante kev:(a) Diagrama de tiempos: pata en apoyo (ĺınea continua), pata en transferencia (ĺınea discontinua); (b) Margen de estabilidad absoluto; (c) Margen cinemático. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58 3.12. Modo de caminar al aumentar la velocidad del robot:(a) Diagrama de tiempos: pata en apoyo (ĺınea continua), pata en transferencia (ĺınea discontinua); (b) Margen de estabilidad absoluto; (c) Margen cinemático. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 3.13. Modo de caminar sobre terreno con zonas prohibidas. Puntos de apoyo de patas delanteras (ćırculo azul), puntos de apoyo de patas centrales (ćırculo rojo) y puntos de apoyo de patas traseras (ćırculo negro). (a) Sin tener en cuenta zonas prohibidas; (b) Teniendo en cuenta zonas prohibidas. . . . . . . . . . . . . . . . . . . . . . . . . 60 3.14. Modo de deriva. Fase 1 (deriva 0o): todas las patas en fase de apoyo (azul). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 3.15. Modo de deriva. Fase 2 (deriva 0o): patas 1, 4, y 5 en fase de apoyo (azul), patas 2, 3 y 6 en transferencia (rojo). . . . . . . . . . . . . 62 3.16. Modo de deriva. Fase 3 (deriva 20o): patas 1, 4, y 5 en fase de apoyo (verde), patas 2, 3 y 6 en transferencia (amarillo). . . . . . . 63 3.17. Modo de deriva. Fase 4 (deriva 20o): todas las patas en fase de apoyo (verde). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 3.18. Modo de deriva. Fase 5 (deriva 20o): patas 2, 3 y 6 en fase de apoyo (verde), patas 1, 4, y 5 en transferencia (amarillo). . . . . . . . . . 65 3.19. Terreno en el que se desplaza el robot con presencia de zonas prohibidas (color rojo) y un área de seguridad (color negro). . . . . 66 ÍNDICE DE FIGURAS XIX 3.20. Transferencia de un tŕıpode evitando pisar sobre zonas prohibidas (secuencia de izquierda a derecha y de arriba a abajo.) . . . . . . . 67 4.1. Sistema de referencia del robot móvil. . . . . . . . . . . . . . . . . 70 5.1. Funcionamiento del algoritmo de localización del filtro de part́ıculas. La figura supone que las muestras están estimando un único parámetro, distribuido en el eje horizontal. Las muestras se representan por ćırculos centrados en el valor del parámetro que definen. El área de los ćırculos representa el peso de cada muestra. 95 6.1. Vector de estado en el sistema de referencia fijo. . . . . . . . . . . . 99 6.2. Velocidad del robot SILO6: velocidad teórica (ĺınea roja) y velocidad estimada (ĺınea azul). . . . . . . . . . . . . . . . . . . . . 100 6.3. Señal de la orientación después de aplicar un filtro de 2o orden: la señal de entrada al filtro (brújula electromagnética) se representa en color azul, la salida para los parámetros del filtro utilizado, γ = 0.6, β = 0.35 y λ = 0.05, se representa en rojo. . . . . . . . . . 101 6.4. Señal de la orientación después de aplicar un filtro de 2o orden: la señal de entrada al filtro (brújula electromagnética) se representa en azul, la salida para los parámetros del filtro γ = 0, β = 0.9 y λ = 0.1 se representa en amarillo, para los parámetros del filtro γ = 0.5, β = 0.35 y λ = 0.15 se representa en verde, para los parámetros del filtro γ = 0.5, β = 0.4 y λ = 0.1 se representa en negro, para los parámetros del filtro γ = 0.6, β = 0.35 y λ = 0.05 se representa en rojo. . . . . . . . . . . . . . . . . . . . . . . . . . . 102 6.5. Inclinómetro: AccuStar II Dual Axis Clinometer. . . . . . . . . . . 104 6.6. Desplazamiento transversal debido a holguras en las articulaciones de las patas del robot. . . . . . . . . . . . . . . . . . . . . . . . . . 105 6.7. Estados del robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . 105 6.8. Evolución del balanceo del cuerpo del robot: “apoyo” (puntos negros), “tŕıpode 1” (puntos rojos) y “tŕıpode 2” (puntos azules). . 106 XX ÍNDICE DE FIGURAS 6.9. Señal del inclinómetro después de aplicar un filtro de 2o orden: la señal de entrada al filtro (inclinómetro) se representa en azul, la salida para los parámetros del filtro γ = 0, β = 0.9 y λ = 0.1 se representa en amarillo, para los parámetros del filtro γ = 0.5, β = 0.4 y λ = 0.1 se representa en verde, para los parámetros del filtro γ = 0.6, β = 0.35 y λ = 0.05 se representa en rojo. a) Cabeceo, b) Balanceo. . . . . . . . . . . . . . . . . . . . . . . . . . 107 6.10. Señal del inclinómetro después de aplicar un filtro de 2o orden: la señal de entrada al filtro (inclinómetro) se representa en color azul, la salida para los parámetros del filtro utilizado, γ = 0.6, β = 0.35 y λ = 0.05, se representa en rojo. a) Cabeceo, b) Balanceo. . . . . 108 6.11. Cambios de estado posibles en el modo de caminar del robot SILO6.109 6.12. Variación del balanceo para el cálculo del umbral de cada estado: “apoyo” (ĺınea negra), “tŕıpode 1” (ĺınea roja) y “tŕıpode 2” (ĺınea azul). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110 6.13. Receptor GPS Trimble 5700 y antena Zephyr. . . . . . . . . . . . . 115 6.14. Incrementos de los datos obtenidos del DGPS (∆Coordenadas UTM) en condiciones estáticas (puntos azules) y valor medio (punto rojo). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 6.15. Histogramas de las coordenadas UTM: (a) Este; (b) Norte. . . . . 117 6.16. Dispositivo utilizado para el estudio del DPGS en condiciones dinámicas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 6.17. Trayectorias teórica de la antena (ĺınea roja) y descrita por el DGPS (ĺınea azul) en ambos sentidos: (a) Ida; (b) Vuelta. . . . . . 120 6.18. Error transversal (ĺınea azul) y error longitudinal (ĺınea roja) en valor absoluto en ambos sentidos: (a) Ida; (b) Vuelta. . . . . . . . 121 6.19. Dispositivo para la calibración geométrica del robot SILO6. . . . . 123 6.20. Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con la odometŕıa (puntos azules) y posiciones estimadas en los puntos de parada con la odometŕıa (puntos rojos). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 6.21. Errores relativos a la distancia recorrida introducidos por la odometŕıa: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 6.22. (a) Brújula electromagnética: 1525 Analog Sensor; (b) Posición de la brújula electromagnética en el robot SILO6. . . . . . . . . . . . 128 ÍNDICE DE FIGURAS XXI 6.23. Dispositivo utilizado para la calibración de la brújula electromagnética.129 6.24. Posiciones utilizadas para calibrar la brújula electromagnética: puntos de parada realizados (azul), origen de la circunferencia (rojo).130 6.25. Salidas del seno y coseno obtenidas de la brújula electromagnética. 132 6.26. Representación de los ángulos obtenidos con la brújula electromagnética y el DGPS en cada punto de parada: los datos recogidos en azul y los datos a los que se ha aplicado la traslación en rojo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 6.27. Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con dead-reckoning (puntos azules) y posiciones estimadas en los puntos de parada con dead-reckoning (puntos rojos). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134 6.28. Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con dead-reckoning (puntos azules) y trayectoria estimada con odometŕıa (puntos rojos). . . . . . . . . 135 6.29. Errores relativos a la distancia recorrida introducidos por dead- reckoning: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 6.30. Trayectoria deseada (ĺınea roja), trayectoria descrita (ĺınea negra) y posición final del robot SILO6 para el experimento EXP-2. . . . 138 6.31. Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con EKF (puntos azules) y posiciones estimadas en los puntos de parada con EKF (puntos rojos). . . . . 140 6.32. Puntos de parada del robot (puntos negros) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning (azul claro), DGPS (verde) y EKF (rojo). . . . . . . . . . . . . . . . . . 141 6.33. Errores relativos a la distancia recorrida introducidos por EKF: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). . . . 142 6.34. Errores en la medida calculada con EKF: para la coordenada X1 (azul), para la coordenada X2 (rojo) y para la coordenada X3 (verde).143 6.35. Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con EKF (puntos azules) y posiciones estimadas en los puntos de parada con EKF (puntos rojos). . . . . 144 XXII ÍNDICE DE FIGURAS 6.36. Puntos de parada del robot (puntos negros) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning - encoders y brújula electromagnética- (azul claro), dead-reckoning -encoders, brújula electromagnética e inclinómetro- (azul oscuro), DGPS (verde) y EKF (rojo). . . . . . . . . . . . . . . . . . . . . . 146 6.37. Errores relativos a la distancia recorrida introducidos por EKF: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). . . . 146 6.38. Errores en la medida calculada con EKF: para la coordenada X1 (azul), para la coordenada X2 (rojo) y para la coordenada X3 (verde).148 7.1. Seguimiento de trayectorias con un robot: a) basado en ruedas y b) basado en patas. . . . . . . . . . . . . . . . . . . . . . . . . . . . 151 7.2. Trayectoria de exploración completa del terreno. . . . . . . . . . . 153 7.3. Estrategia de control de trayectoria. . . . . . . . . . . . . . . . . . 154 7.4. Cambio de trayectoria. . . . . . . . . . . . . . . . . . . . . . . . . . 157 7.5. Simulación 1: (a) Posición del centro geométrico del robot en el plano XY y (b) orientación del robot en función del tiempo. . . . . 158 7.6. Simulación 2: (a) Posición del centro geométrico del robot y (b) orientación del robot. . . . . . . . . . . . . . . . . . . . . . . . . . . 159 7.7. Simulación 3: (a) Posición del centro geométrico del robot y (b) orientación del robot. . . . . . . . . . . . . . . . . . . . . . . . . . . 160 7.8. Simulación 4: (a) Posición del centro geométrico del robot y (b) orientación del robot sin tener en cuenta las zonas prohibidas sobre el terreno. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161 7.9. Simulación 4: (a) Posición del centro geométrico del robot y (b) orientación del robot teniendo en cuenta las zonas prohibidas sobre el terreno. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162 7.10. Modo de caminar sobre terreno sin tener en cuenta zonas prohibidas. Puntos de apoyo de patas delanteras (ćırculo azul), puntos de apoyo de patas centrales (ćırculo rojo) y puntos de apoyo de patas traseras (ćırculo negro). . . . . . . . . . . . . . . . . . . . 163 7.11. Modo de caminar sobre terreno teniendo en cuenta zonas prohibidas. Puntos de apoyo de patas delanteras (ćırculo azul), puntos de apoyo de patas centrales (ćırculo rojo) y puntos de apoyo de patas traseras (ćırculo negro). . . . . . . . . . . . . . . . . . . . 164 ÍNDICE DE FIGURAS XXIII 7.12. Trayectoria descrita y posición final del robot SILO6 para el experimento EXP-3. . . . . . . . . . . . . . . . . . . . . . . . . . . 165 7.13. Puntos de parada del robot (puntos negros), trayetoria a seguir (ĺınea negra) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning -encoders y brújula electromagnética- (azul claro), dead-reckoning -encoders, brújula electromagnética e inclinómetro- (azul oscuro), DGPS (verde) y EKF (rojo). . . . . . 172 7.14. Errores relativos a la distancia recorrida introducidos por la odometria: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. . . . . . . . . . . . . . . . . 173 7.15. Errores relativos a la distancia recorrida introducidos por dead- reckoning (dr1): error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. . . . . . . . . . . . . . . . . 173 7.16. Errores relativos a la distancia recorrida introducidos por dead- reckoning (dr2): error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. . . . . . . . . . . . . . . . . 174 7.17. Errores relativos a la distancia recorrida introducidos por EKF: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. . . . . . . . . . . . . . . . . . . . . . . 174 7.18. Trayectoria de exploración completa del robot SILO6 para el experimento EXP-4. . . . . . . . . . . . . . . . . . . . . . . . . . . 175 7.19. Trayetoria a seguir (ĺınea negra) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning -encoders y brújula electromagnética- (azul claro), dead-reckoning -encoders, brújula electromagnética e inclinómetro- (azul oscuro), DGPS (verde) y EKF (rojo). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176 7.20. Error absoluto transversal introducidos por dead-reckoning (dr1): segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). . . . . . . . . . . . . . . . . . . . . . . . . . 177 XXIV ÍNDICE DE FIGURAS 7.21. Error absoluto transversal introducidos por dead-reckoning (dr2): segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). . . . . . . . . . . . . . . . . . . . . . . . . . 177 7.22. Error absoluto transversal introducidos por dead-reckoning (dr2): segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). . . . . . . . . . . . . . . . . . . . . . . . . . 178 7.23. Error absoluto transversal introducidos por EKF: segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo).178 B.1. Robots caminantes en tareas de detección de minas antipersonas . 194 B.2. Robots caminantes y escaladores desarrollados en el IAI-CSIC. . . 196 B.3. Sistema DYLEMA. . . . . . . . . . . . . . . . . . . . . . . . . . . . 197 B.4. Detector de metales. . . . . . . . . . . . . . . . . . . . . . . . . . . 201 B.5. Cabeza Sensora. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201 B.6. Manipulador de barrido portando la cabeza sensora. . . . . . . . . 203 B.7. Robot caminante SILO6: (a) configuración mamı́fero y (b) configuración insecto. . . . . . . . . . . . . . . . . . . . . . . . . . . 206 B.8. Pares en las articulaciones del robot caminante a lo largo de un ciclo de locomoción empleando tŕıpodes alternados. . . . . . . . . . 207 B.9. Configuración de la pata del robot SILO6. . . . . . . . . . . . . . . 208 B.10.Arquitectura del hardware del SILO6. . . . . . . . . . . . . . . . . 211 B.11.Arquitectura del software del SILO6. . . . . . . . . . . . . . . . . . 213 C.1. Segmento espacio. . . . . . . . . . . . . . . . . . . . . . . . . . . . 216 C.2. Situación de las estaciones monitoras y de la estación experta. . . . 217 C.3. Comparación entre la señal del receptor GPS y la procedente del satélite. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 218 Índice de Tablas 6.1. Desviación estándar de cada modo. . . . . . . . . . . . . . . . . . . 112 6.2. Resultados de las medidas estáticas. . . . . . . . . . . . . . . . . . 118 6.3. Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con odometŕıa (Xo y Yo) y errores relativos a la distancia ∆x, ∆y y ∆. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 6.4. Datos obtenidos en cada punto de parada. . . . . . . . . . . . . . . 131 6.5. Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con dead-reckoning (Xdr y Ydr) y errores relativos a la distancia ∆x, ∆y y ∆. . . . . . . . . . . . . . . . . . . . . . . . . . 135 6.6. Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con EKF (Xekf y Yekf ) y errores relativos a la distancia ∆x, ∆y y ∆. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141 6.7. Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con EKF (Xekf y Yekf ) y errores relativos a la distancia ∆x, ∆y y ∆. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 6.8. Paradas del robot (P ), posiciones estimadas con dr1 (Xdr1 y Ydr1), posiciones estimadas con dr2 (Xdr2 y Ydr2) y diferencia de errores relativos absolutos obtenidos con ∆dr1 y ∆dr2. . . . . . . . . . . . . 147 7.1. Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con odometŕıa (Xo y Yo) y errores ∆x, ∆y y ∆. . . . . . 167 7.2. Paradas del robot (P ), posiciones del robot, posiciones estimadas con dead-reckoning (sin Método de Balanceo) y errores. . . . . . . 168 7.3. Paradas del robot (P ), posiciones del robot, posiciones estimadas con dead-reckoning (con Método de Balanceo) y errores. . . . . . . 169 7.4. Paradas del robot (P ), posiciones del robot, posiciones estimadas con EKF (con Método de Balanceo) y errores. . . . . . . . . . . . . 170 XXVI ÍNDICE DE TABLAS 7.5. Paradas del robot (P ), posiciones estimadas del robot con los dos implementaciones de dead-reckoning y errores. . . . . . . . . . . . 171 B.1. Principales caracteŕısticas del manipulador de barrido. . . . . . . . 203 B.2. Caracteŕısticas del cuerpo del robot caminante SILO6. . . . . . . . 204 B.3. Caracteŕısticas de la pata del robot caminante SILO6. . . . . . . . 205 Caṕıtulo 1 Introducción 1.1. Robots caminantes Los robots caminantes ya han demostrado en un elevado número de ejemplos que constituyen un sistema de locomoción capaz de realizar tareas que los veh́ıculos convencionales no pueden llevar a cabo. Además, han resultado ser un amplio campo de investigación y experimentación en una variedad de disciplinas que incluyen la bioloǵıa, la zooloǵıa, el diseño de prótesis, etc. Desde 1962, año en que se comenzó el desarrollo de la primera máquina caminante conocida (véase la Figura 1.1), hasta nuestros d́ıas, la tecnoloǵıa de robots caminantes se ha desarrollado extraordinariamente produciendo un gran número de diseños (Berns, 2006) y algoritmos de control (generación de modos de caminar, criterios de estabilidad, control y distribución de fuerzas, etc.)1. Sin embargo, sólo unos pocos robots han demostrado su utilidad en aplicaciones reales en la industria y los servicios como, por ejemplo, el robot Dante II (véase la Figura 1.2) desarrollado en la Universidad Carnegie-Mellon, E.E.U.U., para la exploración de volcanes activos (Bares y Wettergreen, 1999); el veh́ıculo ASV (Adaptive Suspension Vehicle) (véase la Figura 1.3) desarrollado en la Universidad del Estado de Ohio, E.E.U.U., como veh́ıculo militar (Waldron y McGhee, 1986; Song y Waldron, 1989), el robot ROWER construido en el Instituto de Automática Industrial - CSIC (véase la Figura 1.4), España, para la soldadura de los dobles fondos de petroleros (Gonzalez de Santos et al., 2000); o la 1Para una introducción general a los robots caminantes, sus oŕıgenes, sus ventajas e inconvenientes y sus aplicaciones potenciales pueden consultarse los textos de Todd (1985), Raibert (1986) y Gonzalez de Santos et al. (2006). 2 Introducción Figura 1.1: General Electric Walking Truck, (E.E.U.U.). máquina Timberjack desarrollada por la compañ́ıa Plustech-Oy, Finlandia (véase la Figura 1.5), como veh́ıculo para tareas forestales, cuyo desarrollo fue adquirido por la multinacional John Deere especializada en veh́ıculos agrarios y forestales. Un ejemplo más recientemente lo podemos encontrar en el robot Roboclimber (véase la Figura 1.6) que se está poniendo a punto por un consorcio europeo para tareas de consolidación de terrenos en construcción civil (Cepolina et al., 2006; Montes et al., 2006). Cada aplicación determina las caracteŕısticas mecánicas de la máquina y sus algoritmos de control. Aśı podemos distinguir entre máquinas para aplicaciones en terrenos relativamente llanos y máquinas que deben desplazarse por terrenos muy irregulares. Máquinas para terrenos practicables (irregulares pero que siempre ofrecen un punto de apoyo) y máquinas para terrenos con zonas prohibidas, determinadas por la existencia de protrusiones, agujeros o áreas donde no deben apoyarse los pies (minas antipersonas, por ejemplo (Gonzalez de Santos et al., 2004b)). Una aplicación en concreto determinará si el robot debe seguir 1.1 Robots caminantes 3 Figura 1.2: Dante II (Universidad Carnegie-Mellon, E.E.U.U.). automáticamente una trayectoria predefinida o, por el contrario, siempre va a estar a la vista del operario y el guiado se va a realizar por éste utilizando los dispositivos de control necesarios y con realimentación visual directa. Finalmente, la precisión de movimientos también va a estar determinada por la aplicación. Si se trata de que el robot se desplace siguiendo una trayectoria por un camino o carretera, una precisión de posicionamiento de un metro puede ser suficiente. Si estamos identificando minas antipersonas o realizando trabajos de topograf́ıa, la precisión centimétrica puede ser incuestionable. Este trabajo de tesis doctoral, que se fundamenta en el diseño y experimentación de algoritmos para la detección y localización de minas antipersonas con robots caminantes, presenta unas caracteŕısticas muy espećıficas determinadas por la aplicación en concreto (Gonzalez de Santos et al., 2007). En particular, se trata de una aplicación en la que confluyen terrenos irregulares 4 Introducción Figura 1.3: The Adaptive Suspension Vehicle - ASV (Universidad del Estado de Ohio, E.E.U.U). (naturales) con zonas prohibidas (presencia de minas antipersonas). Para lograr que un robot móvil realice tareas complejas en este tipo de entornos (entornos no estructurados) es necesario planificar sus acciones a distintos niveles jerárquicos, desde aquellos que determinan la estrategia general para resolver la tarea hasta los que deciden los movimientos más elementales de sus actuadores. Esta división permite descomponer un problema complejo en módulos más sencillos. En primer lugar es necesario generar las estrategias generales de guiado del robot para que éste sea capaz de realizar su tarea de una manera eficiente. En aplicaciones en las que se requiera simplemente posicionar el robot en un punto del terreno, esto equivaldŕıa a planificar una trayectoria que evite los posibles obstáculos y permita, por ejemplo, alcanzar la meta en un tiempo mı́nimo, problema tratado extensamente en la literatura cient́ıfica de las últimas décadas (Lee y Song, 1990). En aplicaciones de rastreo o reconocimiento del terreno, tales como nuestro problema de localización de minas antipersonas, se deben precisar estrategias de exploración completa2 (Acar y Choset, 2001), es decir, que definan trayectorias que garanticen que el robot móvil reconocerá todo el terreno, teniendo en cuenta 2Todos los términos indicados en negrilla (exclúyanse las cabeceras de párrafo y los t́ıtulos de secciones) aparecen definidos en el apéndice A. 1.1 Robots caminantes 5 el alcance y las caracteŕısticas de sus subsistemas (sensores, manipuladores, etc.). Asimismo, es necesario fijar los métodos concretos de control para conseguir que el veh́ıculo siga las trayectorias determinadas mediante esta estrategia general teniendo en cuenta las propiedades del veh́ıculo empleado. Estos métodos serán diferentes en veh́ıculos que ofrezcan distintas posibilidades de movimiento o de maniobrabilidad (capacidad de movimiento omnidireccional, capacidad para girar sobre un punto, etc.) y pueden optimizarse para un tipo determinado de veh́ıculo (basado en ruedas motrices y directrices, basado en orugas en configuración diferencial, máquinas caminantes, etc.). En este sentido las máquinas caminantes constituyen un caso singular, dada su gran maniobrabilidad y libertad de movimientos. También, es necesario determinar los movimientos de los actuadores del robot móvil para que se produzca el avance en la dirección requerida. Este nivel de planificación, que es casi trivial en el caso de robots móviles convencionales con ruedas u orugas, cobra una importancia crucial en el caso de robots caminantes, donde recibe el nombre de modo de caminar. En este tipo de robot, lograr que se produzca el avance en la dirección deseada mientras se mantiene en todo momento la estabilidad y se produce una adaptación a un terreno irregular es un problema complejo. Esto se debe a que las patas pueden cumplir sus funciones de soporte y propulsión de la máquina únicamente de manera intermitente, es decir, alternando estados de soporte y transferencia, por la limitación que imponen sus volúmenes de trabajo. La dificultad para planificar los movimientos de las patas es la contrapartida a la gran movilidad y maniobrabilidad de las máquinas caminantes, que entre otras ventajas permiten un movimiento omnidireccional y la adaptación a terrenos muy irregulares. La planificación de los movimientos de las patas se complica aún más en el caso de que no todo el terreno sea apto para soportar el veh́ıculo, sino que existan ciertas zonas donde no se debe pisar, a las que llamaremos zonas prohibidas. Finalmente, el robot debe conocer en todo momento su posición en un sistema de referencia determinado. Esto es lo que se conoce como localización del robot. Existen diferentes técnicas de localización de robots en exteriores que proporcionan la posición del robot con diferentes precisiones. En el caso de localización de minas antipersonas se exige precisión centimétrica lo que determina drásticamente la técnica de localización para esta aplicación particular. Estos tres aspectos: modos de caminar, localización y seguimiento de trayectorias, se estudian en esta tesis doctoral particularizados para la aplicación espećıfica de detección y localización de minas antipersonas. 6 Introducción Figura 1.4: ROWER (Instituto de Automática Industrial – CSIC, España). Figura 1.5: Timberjack (Plustech-Oy, Finlandia). 1.2. Modos de caminar Lingǘısticamente entendemos por modo de caminar la forma o manera de moverse dando pasos, para lo que los animales utilizan patas. En el campo de los robots caminantes un modo de caminar se define como un patrón repetitivo de posicionamiento de los pies. Si disponemos de un robot de n patas y consideramos dos posibles eventos: levantamiento o apoyo de un pie (2n eventos), el número de patrones diferentes 1.3 Seguimiento de trayectorias 7 que se pueden conseguir está definido por 2n!. Considerando el número de secuencias de eventos, comenzando en una determinada, tendremos (2n− 1)!; es decir, para un hexápodo podemos considerar 39916800 secuencias diferentes. No obstante, teniendo en cuenta que el robot debe tener al menos tres patas en apoyo para garantizar estabilidad estática el número se reduce drásticamente; además, los apoyos de estas tres patas deben cumplir ciertos requisitos geométricos, lo que reduce aún más el número de secuencias estables. La secuencia del modo de caminar no es más que una sucesión de movimientos de patas y cuerpo, simultáneos o secuenciales, que permite al robot desplazarse o cambiar su orientación de forma estable, esto, sin caerse al suelo. En un principio, estas secuencias se inspiraron en la naturaleza, pero poco a poco se fueron introduciendo otras ideas con importantes prestaciones desde el punto de vista de la ingenieŕıa. Aśı, nos encontramos con que los modos de caminar o algoritmos para generar movimiento estable ha sido una de las áreas más activas de la investigación en tecnoloǵıa de robots caminantes. Esta tesis doctoral plantea un nuevo algoritmo para el desplazamiento de robots hexápodos. 1.3. Seguimiento de trayectorias Algunos robots caminantes están maniobrados por operarios embarcados en el robot que le indican en cada instante la dirección y velocidad de movimiento (véanse la Figura 1.1, la Figura 1.3 y la Figura 1.5). Sin embargo, muchas aplicaciones requieren robots capaces de desplazarse sin la intervención de operarios humanos que observen e interpreten las condiciones del entorno y obren en consecuencia. Estos robots deben seguir las indicaciones impuestas por un nivel de control superior en la jerarqúıa del controlador. Esto es lo que se conoce como seguimiento automático de trayectorias. Al seguimiento de trayectorias con robots caminantes se le ha dedicado relativamente poco esfuerzo en comparación con otros aspectos de la locomoción caminante. Se pueden encontrar algunos trabajos sobre este tema como el desarrollado por Orin (1982) que propuso un algoritmo para el seguimiento de una determinada trayectoria utilizando joysticks. Después, adaptó el algoritmo para terreno irregular, pero considerando sólo la inclinación del terreno y zonas prohibidas. Posteriormente, Lee y Song (1990) desarrollaron un método para generar y seguir una trayectoria utilizando modos ondulatorios. Como veremos más adelante, estos modos sólo funcionan en terrenos planos con lo que su utilización es muy limitada. Años más tarde se presentaron algunos resultados 8 Introducción Figura 1.6: Roboclimber (Instituto de Automática Industrial – CSIC, España). para seguimiento de trayectorias con modos discontinuos en cuadrúpedos, lo que facilita las tareas de adaptación al terreno ya que mientras la pata en transferencia está buscando apoyo, el cuerpo del robot está parado (Gonzalez de Santos y Jimenez, 1995). A parte de un generador de modos de caminar estable, el controlador del robot tiene que realizar otras tareas como el control de inclinación y elevación del cuerpo. El control de inclinación es importante para corregir movimientos no deseados del cuerpo del robot debidos a imperfecciones en el mecanismo (flexiones y holguras en las articulaciones de las patas) y los problemas derivados de la compactación del terreno donde pisa el robot. Además, es fundamental para caminar sobre terrenos muy inclinados y para mantener el cuerpo nivelado, lo cual suele ser un requisito básico en veh́ıculos que cargan operarios o pasajeros y en robots que portan algún tipo de sensor. También es importante mantener la altura del cuerpo sobre el suelo para adaptar éste a las irregularidades del terreno. 1.4 Localización de robots 9 Si el controlador del robot garantiza la inclinación/orientación del cuerpo, la altura al suelo y la adaptación al terreno, entonces el seguimiento de trayectorias planas (dos dimensiones) es suficiente para que un robot se desplace por terreno irregular. Un robot caminante es un móvil omnidireccional, por tanto el seguimiento de trayectorias lo puede realizar de diferentes maneras. Por ejemplo, el centro del sistema de referencia del cuerpo (normalmente situado en el centro geométrico del cuerpo) puede seguir una trayectoria manteniendo constante la orientación de su eje longitudinal o manteniendo este eje tangente a la trayectoria. En el primer caso estamos ante lo que se ha definido como modos de deriva y en el segundo caso necesitaremos lo que se ha definido como modos giratorios (véase la Figura 1.7). 1.4. Localización de robots Independientemente del modo de caminar que se utilice, el seguimiento con precisión de trayectorias requiere el conocimiento de la posición y orientación del robot. Este tema se conoce como localización y se ha trabajado intensamente en el campo de los robots móviles (con ruedas y orugas) para su localización tanto en interiores como en exteriores. Las técnicas de localización de robots móviles, en general, pueden aplicarse a robots caminantes; sin embargo, estos tienen unas (a) (b) Figura 1.7: Seguimiento de trayectorias con (a) modos de deriva y (b) modos giratorios. 10 Introducción caracteŕısticas propias que deben tenerse en cuenta para mejorar la precisión en la localización. Existen sistemas de localización local y global con una precisión suficiente para el movimiento de robots en terrenos naturales (no estructurados)¸ pero algunas aplicaciones como la detección y localización de minas antipersonas requieren una precisión más exigente y se impone el uso de técnicas de integración de diferentes medidas sensoriales que mejoran la precisión individual de cada sensor. Para que un móvil pueda seguir una trayectoria es necesario que disponga de una definición precisa de la misma en un sistema de referencia conocido. Pero ante todo, el móvil debe saber dónde está posicionado en el mismo sistema de referencia en que está definida la trayectoria. Las técnicas de localización de robots, normalmente, proporcionan la posición y la orientación del móvil y existen dos tipos básicos: localización relativa y localización absoluta. En la localización relativa se emplean sensores a bordo que proporcionan información incremental de los movimientos del robot. Estas técnicas son sencillas y baratas, pero propensas a acumular errores en la posición y orientación. La localización absoluta se basa en medidas con sensores exteroceptivos; son más complejas, pero más precisas ya que no aparecen acumulación de errores. El tema de la localización de robots en exteriores, en general, está en plena vigencia como acredita la gran cantidad de trabajos cient́ıficos que siguen publicándose anualmente en los congresos internacionales más importantes (IEEE International Conference on Robotics and Automation - ICRA, IEEE/RSJ International Conference on Intelligent Robots and Systems – IROS, International Conference on Advanced Robotics – ICAR, etc.). La localización de robots caminantes en exteriores es un caso particular de la localización general con caracteŕısticas propias que deben ser explotadas convenientemente para mejorar sus prestaciones. Los primeros trabajos de localización de robots caminantes se desarrollaron al principio de la década de los 90 utilizando técnicas dead-reckoning3 (Roston y Krotkov, 1991). Posteriormente se han desarrollado técnicas basadas en sistemas de visión para robots caminantes en interiores con el fin de utilizarlos, básicamente, en competiciones académicas. Recientemente se ha revitalizado la investigación en la localización de robots caminantes en exteriores y han aparecido 3En inglés se define como: “the determination without the aid of celestial observations of the position of a ship or aircraft from the record of the courses sailed or flown, the distance made, and the known or estimated drift”. Debido a que no existe una traducción al español de este término, utilizaremos el vocablo inglés. 1.5 Objetivos de la tesis 11 nuevos trabajos en localización mediante el análisis de dead-reckoning (Mae et al., 2002), incorporación de filtros de Kalman (Gassmann et al., 2005) e incluso se han comenzado a utilizar dispositivos GPS (Global Positioning Systems)(Nonami y Huang, 2001). 1.5. Objetivos de la tesis 1.5.1. Antecedentes El origen de este trabajo de tesis doctoral está en el proyecto DYLEMA (PN I+D+i DPI2001-1595 y DPI2004-05824) que versa sobre la integración de diferentes tecnoloǵıas para la detección y localización de minas antipersonas (Gonzalez de Santos et al., 2004c). Este sistema está formado por: 1. Cabeza detectora de minas que utiliza un detector de minas antipersonas comercial y una serie de sensores de proximidad que informan de la distancia de la cabeza al suelo y de la presencia de objetos en su trayectoria. 2. Brazo manipulador que “barre” el terreno con la cabeza detectora en busca de alarmas potenciales (objetos metálicos) (Gonzalez de Santos et al., 2004a). En este barrido se determina la existencia de alarmas de minas (zonas prohibidas), su posición y tamaño aproximado. Además, se facilita la información de los posibles obstáculos en el camino. 3. Plataforma móvil capaz de desplazar los subsistemas anteriores sobre el campo objeto de la inspección. La plataforma móvil está formada por un robot caminante hexápodo (véase el apéndice B) que debe seguir una trayectoria determinada para asegurar el barrido completo del campo que se inspecciona. Esta trayectoria inicial se puede ver alterada por la presencia de obstáculos en el camino (árboles, maleza, rocas, depresiones, elevaciones, etc.) que serán detectados en tiempo real mediante la cabeza detectora. La trayectoria real del robot será una combinación de la inicial más la impuesta por los obstáculos. 1.5.2. Objetivos globales Esta tesis doctoral se centra en el seguimiento de trayectorias de robots caminantes en lazo cerrado. Esto quiere decir que la trayectoria que debe seguirse 12 Introducción se obtiene de un sistema de rango superior en la jerarqúıa de control que se encarga de la generación de trayectorias siguiendo criterios ajenos a este trabajo. Para el seguimiento de la trayectoria el robot debe saber donde está; esto supone localizar el robot. Conociendo la trayectoria a priori deberá generar los movimientos requeridos para disminuir el error de posición; en este punto se trata de generación de modos de caminar o movimientos coordinados de sus elementos (patas y cuerpo). Se trata, pues, de tres componentes fundamentales en un sistema de control de un robot caminante: 1. Generación de modos de caminar 2. Localización de robots caminantes en exteriores 3. Seguimiento de trayectorias con robots caminantes Los algoritmos presentados son, en general, independientes del robot. Las técnicas de localización y seguimiento de trayectorias son incluso independientes del número de patas del veh́ıculo. Sin embargo, los algoritmos expuestos para la generación de los modos de caminar son exclusivos de robots hexápodos. Aunque son tres los temas principales de este trabajo, consideramos que la generación de modos de caminar es una parte intŕınseca al seguimiento de trayectorias; por esta razón no aparece impĺıcitamente en el t́ıtulo de esta tesis doctoral. En las ocasiones que se impone una particularización para parámetros que dependen del robot se han considerado aquellos del robot SILO6 presentado en el apéndice B. Esta tesis doctoral integra los puntos anteriores y aporta los nuevos resultados teórico-prácticos derivados de los intentos de resolución del problema propuesto. 1.6. Organización de la memoria El caṕıtulo 2 de esta memoria introduce los modos de caminar y presenta sus propiedades con la intención de definir las caracteŕısticas del modo más adecuado para la aplicación que nos ocupa. El caṕıtulo 3 describe este modo de caminar, resalta sus caracteŕısticas más innovadoras y las justifica en simulación y experimentalmente. Los siguientes caṕıtulos se centran en el problema de la localización. El caṕıtulo 4 introduce el problema de la localización de robots en exteriores, el caṕıtulo 5 presenta las bases probabiĺısticas de la fusión sensorial y el caṕıtulo 6 presenta un método de fusión sensorial basada en el filtro de Kalman. La técnica del filtro de Kalman ha sido ampliamente utilizada en la 1.6 Organización de la memoria 13 localización de robots móviles. En este caso, el método tiene en cuenta las peculiaridades de los robots caminantes. El caṕıtulo 7 presenta una técnica de seguimiento de trayectorias con robots caminantes que aglutina el modo de caminar con el sistema de localización y presenta resultados en simulación y experimentales. Finalmente, los resultados más importantes y las conclusiones más significativas de este trabajo se presentan en el apartado de Principales Aportaciones. La memoria se complementa con cuatro apéndices: el apéndice A agrupa las definiciones de los términos que aparecen en el texto; el apéndice B introduce el robot caminante SILO6, utilizado como banco de experimentación; el apéndice C describe el sistema de posicionamiento global como complemento al caṕıtulo 6; y, finalmente, el apéndice D presenta las ecuaciones utilizadas en los cálculos geométricos del modo de caminar desarrollado en el caṕıtulo 3. 14 Introducción Caṕıtulo 2 Modos de caminar 2.1. Generalidades Este caṕıtulo presenta los conceptos básicos que se han empleado en la formulación de modos de caminar para robots. El fin que se persigue es sentar las bases y definir aquellos conceptos y elementos que se emplearán en la formulación y caracterización del modo desarrollado en el caṕıtulo 3, que forma parte del cuerpo principal de este trabajo. En las primeras máquinas caminantes, el operario era el responsable de coordinar los movimientos de cada articulación para producir movimientos estables. Éste era el caso del Iron Mule Train, desarrollado por el Departamento de Defensa de E.E.U.U., cuyas articulaciones eran accionadas a través de cables por un operario que caminaba detrás de la máquina. Otro ejemplo muy significativo es el General Electric Walking Truck, mostrado en la Figura 1.1, cuyos 12 actuadores teńıan que ser coordinados por el operario para producir movimiento estable. Las dificultades que los operarios encontraron para controlar manualmente este veh́ıculo inspiraron a McGhee y Frank (1968) el desarrollo de autómatas de estados finitos para generar automáticamente secuencias estables de movimientos en las patas (Todd, 1985). Estas secuencias se conocen como modos de caminar y han sido objeto de un trabajo de investigación importante en los últimos años. En términos más precisos, un modo de caminar consiste en la localización en el tiempo y el espacio de los apoyos y levantamientos de cada pie coordinados con el movimiento del cuerpo, en sus seis grados de libertad, con el fin de mover el robot de un punto a otro (Song y Waldron, 1989). El sistema locomotor de un robot caminante lo constituye la pata: cadena 16 Modos de caminar cinemática abierta con articulaciones activas que puede posicionar su extremo efectivo (pie) en cualquier punto de su volumen de trabajo. Para la descripción de los modos de caminar, se considera que una pata puede estar en uno de dos estados: apoyo o transferencia. Una pata que está en apoyo contribuye a sustentar y desplazar el robot. Una pata está en transferencia cuando ha abandonado un punto de apoyo y se desplaza levantada buscando un nuevo punto de apoyo (paso). El modo de caminar debe producir el movimiento del robot manteniendo en todo instante el cuerpo sobre las patas. Esto es lo que se conoce como estabilidad del modo de caminar. En esta memoria consideraremos estabilidad estática definida por McGhee y Frank (1968) en los siguientes términos: Definición 2.1: Un robot caminante ideal es estáticamente estable en un instante t si al mantener fijas las posiciones de sus pies y detener, simultáneamente, las velocidades de traslación y rotación del sólido ŕıgido resultante, todos los pies en contacto con el plano de apoyo continúan en apoyo. Esta definición está particularizada para una máquina ideal caracterizada porque la masa de las patas es despreciable respecto al peso total del robot, la posición de los apoyos es controlable, el contacto del pie con el suelo es puntual y el pie puede ejercer una fuerza arbitraria sobre su apoyo pero no ejerce momento alguno. Este concepto de máquina ideal puede ampliarse en (Garcia, 2002). Existe una clasificación detallada (Song y Waldron, 1989) de los modos de caminar que a grandes rasgos se pueden agrupar en: periódicos y no periódicos. Los modos periódicos están caracterizados porque las transferencias de patas, los desplazamientos de cuerpo y los puntos de apoyo en el sistema de referencia del cuerpo del robot se repiten ćıclicamente. En los modos no periódicos tanto la pata que va a efectuar la transferencia como el punto de apoyo del pie se seleccionan en tiempo real dependiendo de la situación de los elementos del robot (patas y cuerpo) y de las condiciones del terreno. Tanto los modos periódicos como los no periódicos pueden a su vez ser continuos o discontinuos. Los modos continuos están caracterizados por el movimiento constante del cuerpo, producido por las patas en apoyo, mientras las otras patas realizan sus transferencias. En los modos discontinuos, el cuerpo permanece inmóvil mientras las patas realizan sus transferencias y sólo se mueve cuando todas las patas están en apoyo. Los modos discontinuos son más fáciles de implementar y presentan algunas caracteŕısticas interesantes (Gonzalez de Santos y Jimenez, 1995); además, ya han demostrado su funcionalidad en algunas configuraciones de máquinas caminantes como el robot AMBLER para el que sus constructores diseñaron un modo discontinuo especial que acuñaron como modo circular (véase 2.2 Modos ondulatorios 17 la Figura 2.1) (Bares y Whittaker, 1989, 1993). 2.2. Modos ondulatorios El primer modo de caminar que se formuló matemáticamente fue el modo ondulatorio (Wave gait) que es el que se observa en mamı́feros, reptiles e insectos a baja velocidad (McGhee y Frank, 1968). Es un modo periódico y continuo. El origen de su nombre está en que las patas se levantan y apoyan en un orden tal que produce la impresión de una onda que se desplaza de atrás hacia delante. Los modos ondulatorios se han formulado matemáticamente y se ha demostrado que son óptimos desde el punto de vista de la estabilidad estática (McGhee y Frank, 1968; Bessonov y Umnov, 1973). En esta sección se formula el modo ondulatorio, que es el modo continuo más usado por robots caminantes naturales y artificiales. Esta formulación del modo está particularizada para una máquina ideal. Para la formulación del modo de caminar son necesarias las siguientes definiciones: Definición 2.2: El factor de apoyo de la pata i, βi, es la fracción del tiempo de ciclo en el cual la pata i está apoyada en el suelo. Si βi es el mismo para todas las patas, el modo es regular. Definición 2.3: La fase relativa de una pata i, φi, es la fracción del tiempo de ciclo que una pata retrasa su instante de apoyo respecto a una pata de referencia. Definición 2.4: La longitud de paso, R, es la distancia que se mueve un pie respecto al cuerpo durante la fase de apoyo. R debe de estar contenido en el volumen de trabajo del pie definido por Rx y Ry (véase la Figura 2.2). Definición 2.5: La distancia entre centros, P, es la distancia entre los centros de las longitudes de paso de dos patas adyacentes. Px es la distancia entre centros de patas adyacentes colaterales y Py si las patas son contralaterales (véase la Figura 2.2). Definición 2.6: La longitud de ciclo, λ, de un modo de caminar es la distancia recorrida por el centro geométrico del cuerpo (CDG) durante un ciclo de locomoción completo. Si el modo de caminar es periódico, entonces λ = R β (2.1) Con estas definiciones, se puede describir matemáticamente el modo ondulatorio mediante ecuaciones que representan las fases de cada pata en función 18 Modos de caminar (a) (b)B Movimiento de la pata Movimiento de la pata Movimiento del cuerpo Figura 2.1: (a) El robot caminante AMBLER (Universidad Carnegie-Mellon, E.E.U.U.) y (b) descripción del modo circular. del factor de apoyo, asumiendo que el volumen de trabajo no se solapa, es decir, R ≤ P . A partir de estas definiciones, Sun (1974) descubrió mediante experimentación numérica que el margen de estabilidad de un modo de caminar simétrico y regular de un robot de 2n patas se maximiza con un modo ondulatorio. La forma general del modo ondulatorio se puede describir matemáticamente mediante ecuaciones que representan las fases de cada pata en función del factor de apoyo. Asumiendo 2.2 Modos ondulatorios 19 Px Px Py Rx Ry Volumen de trabajo del pie Figura 2.2: Definiciones geométricas. que el volumen de trabajo no se solapa, es decir, R ≤ P , el modo ondulatorio queda definido por: φ2m+1 = F (mβ), m = 1, 2, ..., n− 1, 3 2n ≤ β < 1 (2.2) donde F es la función fraccional de un número real X (véase la Definición 2.7) y m denota las patas sucesivas después de la pata 1 sobre el lado izquierdo enumerado desde la parte frontal a la trasera. Definición 2.7. Una función fraccional Y = F (X) de un número real X se define como, Y = { parte fraccional de X si X ≥ 0 1− parte fraccional de |X| si X < 0 (2.3) A partir de la ecuación 2.2 se pueden calcular los modos ondulatorios para animales y máquinas de cuatro, seis y ocho patas. Es decir: 2n = 4, φ3 = β, β ≥ 3/4 (2.4) 2n = 6, φ3 = β, φ5 = 2β − 1, β ≥ 1/2 (2.5) 2n = 8, φ3 = β, φ5 = F (2β), φ7 = F (3β), β ≥ 3/8 (2.6) 20 Modos de caminar El concepto de estabilidad en máquinas caminantes se ha introducido en la Definición 2.1. No obstante, la estabilidad en estas máquinas es un concepto binario. Es decir, una máquina está en un estado estable o inestable. Sin embargo, resulta muy interesante cuantificar el grado de estabilidad (lo cerca que está la máquina de perder la estabilidad) para conocer, por ejemplo, su robustez frente a perturbaciones externas. Tradicionalmente se consideran como medidas de estabilidad el margen de estabilidad, ME, y el margen de estabilidad longitudinal, MEL, cuyas definiciones requieren algunas definiciones intermedias: Definición 2.8: El poĺıgono de apoyo está definido por las proyecciones verticales sobre un plano horizontal de las posiciones de los pies en fase de apoyo. Definición 2.9: El margen de estabilidad, ME (Static Stability Margin, SSM), también conocido com margen de estabilidad absoluto, es la menor distancia desde la proyección vertical sobre el plano horizontal del centro de gravedad de la máquina a los lados del poĺıgono de apoyo (véase la Figura 2.3). Definición 2.10: El margen de estabilidad delantero y el margen de estabilidad trasero son las distancias desde la proyección vertical del centro de gravedad de la máquina sobre el poĺıgono de apoyo al lado delantero y trasero, respectivamente, medidas en la dirección de movimiento. Definición 2.11: El margen de estabilidad longitudinal, MEL, es el menor de los márgenes de estabilidad delantero y trasero (véase la Figura 2.4). Definición 2.12: El margen de estabilidad longitudinal de modo de caminar, o margen de estabilidad de modo de caminar, MEM, para un modo periódico es el mı́nimo margen de estabilidad de un ciclo de locomoción completo. Un modo de caminar es estable si MEM ≥ 0, e inestable si MEM < 0. Con las magnitudes definidas en Definición 2.8-Definición 2.12, Sun (1974) formuló la ecuación del margen de estabilidad longitudinal de un modo ondulatorio para un robot de 2n patas con un factor de apoyo en el rango 1 2 ≤ β < 1 de la siguiente forma: Si β ≥ 1/2, o si β > 2/3 y R ≤ Rb, MEL1 = ( n 2 − 1)P + (1− 3 4β )R (2.7) donde P es la distancia entre centros y R es la longitud de paso y Rb = [β/(3β − 2)]P Si β > 2/3 y R > Rb, 2.2 Modos ondulatorios 21 Polígono de apoyo D1 D2 D3 ME=min(D D D )1 2 3, , C.G. Figura 2.3: Margen de estabilidad de un poĺıgono de apoyo. C.G. D1D2 Polígono de apoyo MEL Trasero MEL Delantero Movimiento del cuerpo MEL=min(D D )1 2, Figura 2.4: Margen de estabilidad longitudinal de un poĺıgono de apoyo. MEL2 = ( n 2 − 1 2 )P + ( 1 4β − 1 2 )R (2.8) Particularizando para el caso de un robot cuadrúpedo, n = 2, la expresión obtenida coincide con la que demostraron McGhee y Frank (1968) para el margen de estabilidad longitudinal de un modo ondulario. Teniendo en cuenta (2.7) y que el modo de caminar es periódico, λ = R/β, se obtiene: 22 Modos de caminar MEL4 = ( β − 3 4 ) λ, para 1 > β ≥ 3 4 (2.9) En el caso de un hexápodo, n = 3, se obtiene la siguiente expresión para el margen de estabilidad longitudinal estático de un modo ondulatorio a partir de (2.7): MEL6 = 1 2 P + (1− 3 4β )R (2.10) Considerando que el modo ondulatorio es simétrico, la definición del modo se puede simplificar definiendo las fases de las patas para solo un lado del robot. Para el otro lado, una fase de pata es la misma que la fase de su pata contralateral incrementada en medio ciclo. También es posible evitar definir φ1 porque siempre se define como cero. De esta forma, el modo ondulatorio para un hexápodo se puede definir como: φ3 = β φ5 = 2β − 1, paraβ ≥ 1 2 (2.11) El diagrama de tiempos es una herramienta de análisis importante para el estudio de los modos de caminar continuos. Está formado por tantas ĺıneas horizontales como número de patas tiene el robot (véase la Figura 2.5). El tramo de ĺınea continuo indica el tiempo en el que cada pata está en la fase de apoyo. El tramo discontinuo indica la fase de transferencia de la pata en la que ésta se desplaza levantada en busca de un nuevo punto de apoyo. El principio del tramo continuo indica el instante de apoyo del pie y el final indica el instante de levantamiento. La Figura 2.5(b) muestra el diagrama de tiempos para un modo ondulatorio con factor de apoyo β = 5/6. En esta figura se puede observar como la secuencia de transferencia de las patas es: 6-4-2-5-3-1. Es decir, el orden en el que se transfieren las patas va de atrás hacia delante y de derecha a izquierda. La Figura 2.5(a) muestra la numeración de las patas siguiendo un criterio ampliamente aceptado. Un modo de caminar para hexápodos, que es ondulatorio desde el punto de vista matemático, pero que resulta ser singular, es el modo conocido como tŕıpodes alternados. El modo de tŕıpodes alternados para hexápodos está caracterizado por el apoyo secuencial de dos grupos de tres patas (tŕıpodes) formados por la pata central de un lado y las patas frontal y trasera del otro 2.2 Modos ondulatorios 23 Pata 1 Pata 3 Pata 5 Pata 2 Pata 4 Pata 6 x 1 3 5 2 4 6 1 3 5 2 4 6 Apoyo del pie Levantamiento del pie Pata en apoyo Pata en transferencia Tiempo Tiempo (a) (b) (c) Figura 2.5: Diagramas de tiempos: (a) Numeración de patas en un robot, (b) Diagrama de tiempos de un modo ondulatorio β = 5/6. (c) Diagrama de tiempos de un modo ondulatorio β = 1/2 (tŕıpodes alternados). lado1. Este modo proporciona la máxima velocidad que un robot hexápodo puede alcanzar (Song y Waldron, 1989). Como las máquinas caminantes estáticamente estables son generalmente muy lentas, éste es el modo que utilizan la gran mayoŕıa de robots hexápodos que se han construido. Además, presenta la ventaja de resultar muy estable y fácil de implementar, razones por la que se utiliza con profusión. La Figura 2.5(c) muestra el diagrama de tiempos de un modo de tŕıpodes alternados. 1Se considera modo singular aquel en que aparecen simultáneamente apoyos o levantamientos de varios pies. El modo basado en tŕıpodes alternados es singular porque el instante en que se apoyan los pies de un tŕıpode coinciden con el instante de levantamiento de los pies del otro tŕıpode. 24 Modos de caminar 2.3. Modos de deriva Los modos de caminar básicos desplazan el robot a lo largo del eje longitudinal del cuerpo. Sin embargo, una máquina caminante es intŕınsecamente omnidireccional. Es decir, puede moverse en cualquier dirección simplemente cambiando la secuencia de movimiento de sus patas y los puntos de apoyo de sus pies. Por tanto, puede desplazarse a lo largo de una recta que forme un determinado ángulo con la dirección del eje longitudinal de su cuerpo, por ejemplo. En este caso, el modo resultante se conoce como modo de deriva (Crab gait). Las primeras formulaciones de modos de deriva las llevó a cabo Orin (1982) en la Universidad del Estado de Ohio. Después, Kumar y Waldron (1989), de la misma universidad, desarrollaron un modo ondulatorio de deriva para hexápodos. Años más tarde, Zhang y Song (1990) formularon el modo ondulatorio de deriva para cuadrúpedos. Todos estos modos están formulados para terreno llano, por lo que se han formulado diversas adaptaciones de estos modos a terreno irregular con diferente éxito. Las propuestas han consistido en la incorporación de sensores para detección del terreno y la modificación de algunos parámetros de los algoritmos conocidos. Por ejemplo, Kumar y Waldron (1989) propusieron un modo ondulatorio modificado que se adaptaba automáticamente a unos apoyos para los pies previamente seleccionados. Se trata, básicamente, de modificar el factor de apoyo, la fase de pata, la longitud de paso y el tiempo de ciclo del modo de caminar ondulatorio. Otros autores han propuesto una estrategia de control que automáticamente adapta un modo de caminar ondulatorio con deriva, formulado para terreno plano, a un terreno irregular adaptando los parámetros del modo en tiempo real (Jimenez y Gonzalez de Santos, 1997). 2.4. Modos discontinuos Los modos discontinuos se caracterizan por el movimiento secuencial de las patas y el cuerpo (Gonzalez de Santos y Jimenez, 1995). Una pata se transfiere cuando todas las demás están en apoyo y paradas. El cuerpo es propulsado por todas las patas en apoyo y moviéndose simultáneamente, y por supuesto, manteniendo sus puntos de apoyo. Se debe tener en cuenta diferentes aspectos para generar modos periódicos discontinuos (véase la Figura 2.6). 1. Si una pata que se encuentra en la fase de apoyo alcanza el ĺımite anterior de su volumen de trabajo (posición ĺımite trasera, PLT), esta pata debeŕıa 2.5 Modos libres 25 cambiar a la fase de transferencia para posicionarse en el ĺımite posterior de su volumen de trabajo (posición ĺımite delantera, PLD). 2. El cuerpo es propulsado hacia delante con todas las patas apoyadas sobre el suelo. Después de un movimiento del cuerpo, como mı́nimo una pata debeŕıa permanecer en su ĺımite cinemático anterior para llevar a cabo una fase de transferencia en el próximo movimiento de pata. 3. Cuando se transfiere una pata, las patas delanteras y traseras del otro lado del robot debeŕıan posicionarse de forma que después del posicionamiento de la pata transferida el CDG esté situado dentro del triángulo formado por las ĺıneas que conectan la pata transferida con las patas delanteras y traseras del otro lado del robot. De esta forma es posible levantar otra pata mientras se mantiene la máquina estable. 4. La secuencia de las patas debeŕıa ser periódica; esto permitirá unir varios ciclos de locomoción para configurar una trayectoria. 2.5. Modos libres Todos los modos de caminar considerados hasta ahora pertenecen a la clase de modos periódicos. Como ya se ha mencionado, estos modos están caracterizados porque las transferencias de patas, los desplazamientos de cuerpo y los puntos de apoyo en el sistema de referencia del cuerpo del robot se repiten ćıclicamente (patrones fijos en cada peŕıodo). Sin embargo, los robots caminantes están intŕınsecamente dotados para realizar movimientos no periódicos, de modo similar a los que emplean los mamı́feros en terrenos muy abruptos que deciden en cada instante qué pata mover y dónde apoyarla en función del entorno que divisan, y se olvidan de la periodicidad en aras de la estabilidad. Estos modos se denominan modos libres (Free gaits). Los primeros modos libres se deben a McGhee y Iswandhi (1979), quienes diseñaron modos de caminar para hexápodos basándose en un algoritmo no periódico formulado anteriormente por Kugushev y Jaroshevskij (1975). Este método se basa en la selección de una secuencia de puntos de apoyo para los pies que permite al robot caminar sobre un terreno que contiene ciertas áreas en las que no se pueden apoyar los pies por diferentes causas (elevaciones, agujeros, etc.). De estas áreas, denominadas zonas prohibidas, el controlador del robot 26 Modos de caminar 1 3 4 5 6 7 2 1 3 4 5 6 7 2 A B C D E F Figura 2.6: Modos discontinuos. 2.6 Modos giratorios 27 tiene un conocimiento aprioŕıstico; es decir, un sistema sensorial o un sistema de información le comunican con anterioridad su posición y sus dimensiones. El terreno se divide, por tanto, en zonas prohibidas y permitidas. Este modo de caminar intenta maximizar el número de patas en transferencia, por tanto, sólo resulta adecuado para hexápodos ya que los cuadrúpedos sólo pueden mantener una pata en transferencia en movimientos estáticamente estables que son los que nos ocupan. Además, no considera las posibles irregularidades del terreno y ha sido comprobado solamente en simulación. Algunos años más tarde, Hirose (1984) propuso un modo libre espećıfico de robots cuadrúpedos con resultados muy positivos, pero únicamente se documentaron resultados de simulaciones. Este modo también consideraba el terreno dividido en zonas permitidas y prohibidas y sigue adoleciendo de capacidad de adaptación a terrenos irregulares. Recientemente se han presentado modos libres para cuadrúpedos tanto continuos como discontinuos con adaptación al terreno y teniendo en cuenta zonas prohibidas (Estremera y Gonzalez de Santos, 2002; Estremera et al., 2005). 2.6. Modos giratorios Los modos giratorios (Turning gaits) son aquellos que cambian constantemente la dirección del eje longitudinal del cuerpo del robot. En la literatura sobre este tema se pueden encontrar que, básicamente, estos modos se dividen en modos circulares (radio no nulo) y modos rotatorios2(radio nulo). Los modos circulares (Circling gaits) son aquellos que desplazan el cuerpo siguiendo una circunferencia centrada en un determinado punto (véase la Figura 2.7), los modos rotatorios (Spinning gaits) son aquellos que giran el cuerpo alrededor del eje z del sistema de referencia del cuerpo (alineado con la aceleración de la gravedad y opuesto a ella) (véase la Figura 2.8). Los modos citados son básicos para la generación de trayectorias, mientras que los modos rotatorios se usan principalmente para reposicionar el robot. 2El Diccionario de la Lengua de la Real Academia Española define rotar como dar vueltas alrededor de un eje; y define girar como mover una figura o un objeto alrededor de un punto o de un eje. 28 Modos de caminar Figura 2.7: Modos circulares. Figura 2.8: Modos rotatorios. Caṕıtulo 3 Generación de modos de caminar libres para robots hexápodos 3.1. Introducción En este caṕıtulo se describen los algoritmos empleados para determinar los movimientos de las patas de un robot caminante hexápodo de manera que se produzca una locomoción estable y un desplazamiento controlado del veh́ıculo, es decir, para generar el modo de caminar. Como ya se apuntó en la sección 1.1, para lograr la ejecución de una tarea compleja tal como la exploración completa de un terreno natural con una máquina caminante son necesarios distintos niveles de control, entre los que se encuentran: a) la planificación de trayectoria de exploración completa, b) el control de seguimiento de trayectoria y c) la planificación del modo de caminar. Este caṕıtulo describe el nivel de decisión más bajo dentro de este esquema en el que se obvian las acciones de control elementales como el control de posición o velocidad de las articulaciones. Este es el nivel de control más caracteŕıstico y distintivo de las máquinas caminantes, encargándose de resolver cuestiones que pueden ser triviales en el caso de los veh́ıculos convencionales. Por ejemplo, en los veh́ıculos con ruedas u orugas existe una relación muy sencilla entre los movimientos de los actuadores y el movimiento del veh́ıculo. Por el contrario, en una máquina caminante esta relación es muy compleja, siendo preciso coordinar un gran número de actuadores para lograr una locomoción práctica. Además, 30 Generación de modos de caminar libres para robots hexápodos la estabilidad de los veh́ıculos convencionales se garantiza de manera pasiva (dentro de unos ĺımites de funcionamiento, por ejemplo de inclinación del terreno, aceleración normal, etc.), mientras que en una máquina caminante debe garantizarse activamente mediante un sistema de control. Finalmente, en los veh́ıculos con ruedas no existe la limitación cinemática que obliga a las patas a funcionar alternando un estado en el que contribuyen a la estabilidad y la propulsión (soporte) y un estado en el que únicamente se persigue recuperar un estado útil para volver a soportar y propulsar la máquina (transferencia). Además de resolver todas estas cuestiones, exclusivas de las máquinas caminantes, el modo de caminar es el nivel responsable de lograr que el robot posea unas propiedades superiores a las de los veh́ıculos convencionales, como la posibilidad de movimiento omnidireccional, una adaptabilidad superior a terrenos irregulares o la posibilidad de avanzar por terrenos con zonas prohibidas, es decir, con zonas donde no se debe pisar. Aunque los métodos aqúı presentados son generales para robots caminantes hexápodos, se han considerado especialmente los requerimientos de una aplicación de desminado humanitario que es la que nos ocupa en esta tesis doctoral. Entre estos requerimientos destacan: la movilidad sobre terrenos con zonas prohibidas, el guiado con precisión del robot a lo largo de trayectorias predefinidas y una gran capacidad de adaptación a terrenos naturales. Por otra parte, los cálculos y simulaciones se han particularizado para el robot SILO6 (véase el apéndice B). 3.2. Planteamiento general del modo de caminar Un modo de caminar es la planificación de los movimientos de las patas del robot para que se origine una locomoción estable en la dirección deseada. En la literatura cient́ıfica se han propuesto numerosos métodos, en ocasiones muy dispares para resolver este dif́ıcil problema. Los modos de caminar periódicos (Bessonov y Umnov, 1973) fueron los primeros en formularse pero mostraron pronto sus limitaciones para adaptarse a terrenos irregulares y trayectorias cambiantes. Por esta razón, ya en la década de los 70, se comenzó a desarrollar modos no periódicos, conocidos como modos de caminar libres, capaces de decidir el orden en que se producen las transferencias de sus patas, los puntos de apoyo de los pies y los movimientos del cuerpo en función de las caracteŕısticas del terreno, de la trayectoria a seguir y del estado de la máquina. Desde entonces se ha propuesto una gran variedad de métodos para generar modos libres, basándose en reglas heuŕısticas (Kugushev y Jaroshevskij, 1975; McGhee y Iswandhi, 1979; 3.2 Planteamiento general del modo de caminar 31 Hirose, 1984; Bai et al., 1999), en métodos de búsqueda (Pal y Jayarajan, 1991; Pal y Kar, 2000; Pack y Kang, 1999; Eldershaw y Yim, 2001), en generadores centrales de patrones de movimiento (Lewis y Bekey, 2002; Fukuoka et al., 2003), en métodos de aprendizaje (Porta y Celaya, 2001) o en estudios biológicos de insectos (Dean et al., 1999; Delcomyn, 1999), por citar algunos ejemplos. Sin embargo, hasta el momento ninguno de estos métodos se ha mostrado plenamente efectivo a la hora de controlar con flexibilidad una máquina caminante en terreno no estructurado. En los métodos basados en reglas heuŕısticas los movimientos de las patas del robot se deciden en función de un conjunto de reglas diseñadas por un humano. A pesar de ser los primeros en proponerse, los métodos basados en movimientos cuasi-periódicos o en reglas heuŕısticas son los únicos en los que se conf́ıa actualmente a la hora de mover máquinas caminantes a una escala práctica, ya que son los únicos capaces de planificar con precisión los movimientos de las patas y de garantizar estabilidad medida con criterios f́ısicos. Por estas razones, a la hora de diseñar un modo de caminar para el robot SILO6 se ha optado por este tipo. La potencia de los actuadores limita la velocidad de los robots caminantes. Normalmente, esta velocidad es baja y por ello se ha optado por un modo de caminar estáticamente estable. Como ya se avanzó en el caṕıtulo 1, una máquina que emplee este criterio de estabilidad puede mantenerse estable moviendo sus patas a velocidades arbitrariamente bajas; por otro lado, la máquina será más estable cuanto más bajas sean las aceleraciones y las inercias de las patas y del cuerpo, no contempladas en este criterio. Para ordenar los levantamientos y apoyos de las patas del robot se ha optado por un modo de caminar basado en tŕıpodes alternados presentado en la sección 2.2. Como ya se ha adelantado, en este tipo de modos de caminar para hexápodos las patas se agrupan en dos tŕıpodes, cada uno de ellos formado por la pata delantera y trasera de un lado y la pata central del lado contrario. Las patas de ambos tŕıpodes se levantan y se apoyan simultáneamente y las transferencias se alternan en el tiempo. La principal ventaja de este modo de caminar es que es el modo estáticamente estable para hexápodos con el que se puede alcanzar una mayor velocidad (Song y Waldron, 1989). Además, al ser un caso particular de modo ondulatorio se puede demostrar matemáticamente que es óptimo desde el punto de vista de la estabilidad, dada una velocidad de avance. Por otra parte, empleando este método se limita drásticamente el enorme número de posibilidades que ofrece un robot hexápodo para planificar los movimientos de sus patas. 32 Generación de modos de caminar libres para robots hexápodos Con un modo basado en tŕıpodes alternados se simplifica considerablemente la planificación de los instantes de levantamiento y apoyo de las patas, mientras que la planificación del orden de levantamiento de las patas se hace trivial. Teniendo en cuenta la aplicación a la que está destinado, el modo de caminar generado debe ser apropiado para realizar tareas de seguimiento de trayectorias. Por tanto, el modo de caminar debe garantizar, siempre que sea posible, que el centro de gravedad (CDG) de la máquina sigue la trayectoria básica (rectiĺınea o circular) indicada por un módulo superior de control, sin alterar este comportamiento para aumentar la estabilidad o con cualquier otro fin. Para facilitar el seguimiento de trayectorias se han implementado dos modos de caminar básicos: un modo de deriva (omnidireccional) y un modo de giro. En cuanto a la capacidad de adaptación a terrenos irregulares, se han impuesto dos nuevos requerimientos: por un lado, el robot debe ser capaz de adaptarse a las irregularidades sin necesidad de información previa acerca del terreno; por otro lado, el modo de caminar debe ser capaz de aprovechar la información dada por sensores exteroceptivos para mejorar su adaptación. Esto incluye la posibilidad de planificar la búsqueda de apoyos en terrenos con zonas prohibidas (por ejemplo minas antipersonas), y en función del perfil del terreno (por ejemplo para evitar depresiones, discontinuidades, etc.). Finalmente, se ha adoptado como criterio general en el algoritmo la optimización de la estabilidad y velocidad. Esto se ha tenido en cuenta especialmente en la búsqueda de apoyos, donde queda patente la necesidad de llegar a un compromiso entre ambas propiedades. La planificación del modo de caminar se dividirá en tres módulos distintos encargados de planificar aspectos distintos de la locomoción: 1. Planificador de levantamiento de patas, descrito en la sección 3.7. 2. Planificador de movimiento del cuerpo, que se define en la sección 3.8. 3. Planificador de apoyos, presentado en la sección 3.9. Adicionalmente, en la sección 3.10 se describe brevemente cómo adaptar este algoritmo para generar modos de giro. Las definiciones básicas necesarias para formular el modo de deriva se presentan previamente en las secciones 3.3 a 3.6. Por último, en la sección 3.11 se presentan las simulaciones que prueban la validez del método y en la sección 3.12 la validación experimental. 3.3 Definiciones de estabilidad 33 3.3. Definiciones de estabilidad Para cuestiones relativas a la estabilidad se considera que las patas del robot se agrupan en dos tŕıpodes, formados cada uno de ellos por la pata delantera y trasera de un lado y la pata central del lado contrario (los sub́ındices D, C y T denotan pata delantera, central y trasera, respectivamente, de un tŕıpode, véase la Figura 3.1). Durante la locomoción, los posibles estados de los tŕıpodes serán en apoyo o en transferencia. Podrán estar ambos tŕıpodes en apoyo pero nunca ambos en transferencia al mismo tiempo. En adelante también se empleará el término tŕıpode para designar la terna de puntos en los cuales se encuentran los pies de ese tŕıpode. En los cálculos relativos a estabilidad se considera además que el cuerpo del robot permanece nivelado y que su centro de gravedad (CDG) está situado en el centro geométrico de la máquina. Para formular el modo de caminar se definen las siguientes magnitudes relacionadas con la estabilidad: Definición 3.1: El margen de estabilidad absoluto mı́nimo, MEmin, es el valor ĺımite por debajo del cual no debe caer el margen de estabilidad absoluto (véase la Definición 2.9 y la Figura 3.1) para garantizar la estabilidad de la máquina a pesar de los errores en la nivelación, las aceleraciones de las patas, las flexiones y holguras de las articulaciones, etc. Definición 3.2: El punto de inicio de transferencia, I(PD, PC, PT , Re), y el punto de fin de transferencia, T(PD, PC, PT , Re), se definen como: Sea un tŕıpode de apoyo situado en PD, PC , PT , y un tŕıpode que va a realizar una transferencia, y sea Re la distancia que se desplazará el cuerpo durante esta transferencia. Se definen los puntos de inicio, I, y de fin, T, de transferencia mediante las siguientes condiciones: 1. Los puntos I y T están situados sobre la trayectoria τ (véase la Figura 3.2). Es decir: I(PD, PC , PT , Re)ετ (3.1) T (PD, PC , PT , Re)ετ (3.2) donde τ es la trayectoria a seguir por el robot. 2. La distancia entre los puntos I y T es Re (véase la Figura 3.2). Es decir: δPP (I(PD, PT , PC , Re), T (PD, PC , PT , Re)) = Re (3.3) 34 Generación de modos de caminar libres para robots hexápodos P P P ME Q Q Q Trayectoria del robot CDG Pie en apoyo Pie en transferencia Trípode 2 (en transferencia) Trípode 1 (en apoyo) Ángulo de deriva, Y X C C DD T T Sistema de referencia del robot (eje longitudinal) a Figura 3.1: Definiciones básicas. donde δPP (m,n) es la distancia eucĺıdea entre dos puntos m y n. 3. Las distancias del punto I a la recta PT PC y del punto T a la recta PDPC son iguales (véase la Figura 3.2). Es decir: δPR(I(PD, PC , PT , Re), PDPC) = δPR(T (PD, PC , PT , Re), PCPT ) (3.4) donde δPR(p,mn) es la distancia eucĺıdea de un punto p a una recta que pasa por los puntos m y n. Como se puede apreciar geométricamente en la Figura 3.2, si se comienza a realizar la transferencia cuando el CDG está situado en I(PD, PC , PT , Re) y durante la transferencia el cuerpo se desplaza lo previsto (Re), entonces la estabilidad será óptima durante la transferencia. Definición 3.3: El recorrido de inicio de transferencia L(PD, PC , PT , Re) se define por la siguiente expresión: 3.3 Definiciones de estabilidad 35 P P P R C D T Trayectoria del robot, CDG MEt(P ,P ,P ,R )C T eD Trípode de apoyo Trípode a transferir e MEt(P ,P ,P ,R )C T eD T(P ,P ,P ,R )C T eD I(P ,P ,P ,R )C T eD t L(P ,P ,P ,R )C T eD Figura 3.2: Definiciones relacionadas con la estabilidad. L (PD, PC , PT , Re) = δPP (CDG, I (PD, PC , PT , Re)) (3.5) Es decir, es la distancia del CDG al punto de inicio de transferencia I(PD, PC , PT , Re). Dado un tŕıpode de apoyo en los puntos PD, PC , PT , y un tŕıpode que va a realizar una transferencia, L(PD, PC , PT , Re) es la distancia que tiene que desplazarse el cuerpo antes de iniciar la transferencia para que la estabilidad sea máxima durante la misma. En esta definición se asume que durante la transferencia el cuerpo se desplaza Re. Definición 3.4: El margen de estabilidad de transferencia, MEt(PD, PC , PT , Re), se define como la siguiente distancia: 36 Generación de modos de caminar libres para robots hexápodos MEt (PD, PC , PT , Re) = δPR (I (PD, PC , PT , Re) , PCPT ) (3.6) Alternativamente, también se puede definir como: MEt (PD, PC , PT , Re) = δPR (T (PD, PC , PT , Re) , PDPC) (3.7) ya que ambas distancias son iguales, por definición. Es decir, es el margen de estabilidad absoluto que habrá en el instante de inicio y de fin de una transferencia durante la cual el cuerpo se desplaza Re y que se comienza a realizar en un instante tal que se optimice la estabilidad. Todos los cálculos para obtener el margen de estabilidad de transferencia se presentan en el apéndice D. 3.4. Modelo del terreno El modelo del terreno consiste en un mapa de alturas sobre el cual se pueden especificar zonas prohibidas respecto a un sistema de referencia. Gracias al sistema de localización utilizado (véase el caṕıtulo 6) se supone que, dadas las coordenadas x e y de un punto del terreno referido al sistema de referencia del robot (véase la Figura 4.1), el controlador puede conocer su coordenada z estimada HTM (x, y), también en el sistema de referencia del robot. La información disponible acerca del terreno proviene de sensores exteroceptivos y está afectada por un cierto grado de imprecisión en la medida. Otra posibilidad es que no se disponga de datos sensoriales, sino que se estime la altura del terreno a partir de las posiciones de las patas en apoyo, o bien que simplemente se presuponga que el terreno tiene una cierta altura, inclinación y rugosidad. En cualquier caso se considera que la altura real de un punto del terreno, HTR(x, y), cumple la condición (Estremera y Gonzalez de Santos, 2005): HTM (x, y)− IT < HTR(x, y) < HTM (x, y) + IT (3.8) donde IT es la incertidumbre de altura del terreno, la cual depende de la calidad de la información disponible del terreno (véase la Figura 3.3). Las zonas prohibidas se corresponden con zonas del terreno que no se pueden pisar por razones externas a la locomoción (es decir, especificadas por el usuario o por un nivel superior de la jerarqúıa del controlador) y por aquellos puntos cuya altura estimada HTM (x, y) no permite la correcta transferencia de una pata hasta ellos. 3.4 Modelo del terreno 37 0 50 100 150 200 250 300 350 400 0 20 40 60 80 100 120 140 160 180 200 P 0 P 1 P 2 P 3 Fase 2 Fase 1 Fase 3 Terreno estimado Terreno real Trayectoria de transferencia I T I T H 1 H 3Incertidumbre en la estimación del terreno Punto de apoyo real Punto de apoyo estimado X Z 0 50 100 150 200 250 300 350 400 0 50 0 20 40 60 80 100 120 140 160 180 200 P 0 P 1 P 2 P 3 Fase 2 Fase 1Fase 1Fase 1Fase 1 Fase 2 Fase 3 Terreno estimado Terreno real Trayectoria de transferencia I T I T H 1 H 3 Incertidumbre en la estimación del terreno Punto de apoyo real Punto de apoyo estimado X Y Z ( a ) ( b ) Figura 3.3: Modelo del terreno y trayectoria de transferencia vista desde un sistema de referencia externo: (a) en 3-D, (b) vista lateral. 38 Generación de modos de caminar libres para robots hexápodos 3.5. Definición del volumen de trabajo Para simplificar la planificación del modo de caminar se considerará un volumen de trabajo definido dentro del volumen de trabajo real de la pata. Este volumen de trabajo puede ser de cualquier forma apropiada para ajustarse al volumen real, siempre que esté limitado por dos planos horizontales y por superficies verticales. Esta condición garantiza que si el pie se encuentra dentro del volumen siempre podrá realizar sus movimientos de levantamiento y apoyo entre dos alturas fijas. En la Figura 3.4 se muestra el volumen de trabajo seleccionado en el caso del robot SILO6 con patas de tipo insecto, y que se empleará en las simulaciones y ejemplos presentados. 3.6. Definición de trayectorias de transferencia de las patas La forma de las trayectorias de transferencia determina ciertos aspectos de la planificación de levantamientos y apoyos, por ello es necesario definirlas previamente. En un modo de caminar continuo es necesaria una cierta coordinación entre los movimientos del cuerpo y de la pata para que la transferencia de la pata se produzca de manera suave y precisa. La velocidad horizontal del pie con respecto al suelo debe ser nula en el instante inicial y final Trayectoria del robot Volumen de trabajo de la pata Ángulo de deriva Eje longitudinal Rmin Rmax HvRecorrido cinemático de P, C(P) R1 P ζ Figura 3.4: Recorrido cinemático y volumen de trabajo de la pata. 3.6 Definición de trayectorias de transferencia de las patas 39 de la trayectoria de transferencia con el fin de evitar deslizamientos. Alrededor de estos instantes, dependiendo de la flexibilidad de la estructura de la máquina y de la sensibilidad del sistema de detección del terreno, existirá un intervalo de tiempo en el que se mantendrá el contacto pie-suelo. Por otro lado, la trayectoria empleada debe garantizar que la proyección horizontal del punto en que se apoye finalmente el pie mantenga su posición relativa con respecto a las otras patas, con independencia de la altura exacta del apoyo, con el fin de garantizar las condiciones de estabilidad prescritas por el planificador de apoyos. Teniendo en cuenta estos requerimientos se ha definido el perfil de transferencia que se presenta a continuación. 3.6.1. Definición de la trayectoria de un pie en transferencia En la transferencia el pie se moverá desde el punto de apoyo en el terreno P0 (véase la Figura 3.5) hasta un nuevo punto de apoyo en el terreno P ′ 0 (ambos referidos al sistema de referencia del robot). La trayectoria de transferencia empleada para trasladar el pie de un apoyo a otro consta de tres fases, en cada de las cuales se ejecuta una trayectoria rectiĺınea (véanse la Figura 3.3, la Figura 3.5 y la Figura 3.6). La fase 1 se ejecuta cuando el planificador de levantamiento de patas aśı lo decide. Una vez que se ha completado la fase 1, el planificador de apoyos decide el nuevo punto al que ha de moverse la pata. Finalmente, en función de lo decidido por el planificador de apoyos se ejecutan las fases 2 y 3 de la transferencia. Fase 1: En esta fase el pie se levanta una altura H1 desde su antiguo punto de apoyo siguiendo una trayectoria vertical en el sistema de referencia externo (véase la Figura 3.3). El valor de la constante H1 se prefija de manera que el pie en transferencia se separe suficientemente del suelo, a pesar de las flexiones producidas en la estructura del robot y las deformaciones en el suelo al cambiar la distribución de fuerzas, y teniendo en cuenta la irregularidad del terreno. Respecto al sistema de referencia del robot (véanse la Figura 3.5 y la Figura 3.6) el pie se mueve con una velocidad horizontal dada por el planificador de movimiento del cuerpo (véase la sección 3.8), y con una velocidad vertical V1. En general durante esta fase el cuerpo se desplaza una distancia R1: R1 = VCHH1/V1 (3.9) donde VCH es la velocidad horizontal del cuerpo. Si el pie llegase al ĺımite de su volumen de trabajo, se detendŕıa y finalizaŕıa esta fase. 40 Generación de modos de caminar libres para robots hexápodos Fase 2: En esta fase el pie se desplaza hasta situarse sobre el punto del terreno que será el nuevo punto de apoyo, a una altura H3/2 por encima de la altura estimada de éste, donde H3/2 > IT (IT , incertidumbre de altura del terreno, véase la Figura 3.3). En el sistema de referencia del robot el pie se mueve en ĺınea recta desde el punto P1 en el cual terminó la Fase 1, hasta el punto F calculado por el planificador de apoyos, con una velocidad v cuyas tres componentes son: vx = V2 vy = (P1y − Fy) V2/(P1x − Fx) vz = (P1z − Fz) V2/(P1x − Fx) (3.10) La distancia recorrida por el cuerpo durante esta fase, R2, viene dada por la siguiente expresión: 0 50 100 150 200 250 300 350 400 -100 -50 0 0 50 100 150 200 250 Fase 1 Fase 2 Fase 3 P 3 P 1 P 2 P 0 P' 3 P' 2 P' 1 P' 0X Y Z Apoyo actual Nuevo apoyo F Nuevo apoyo en el terreno F' Figura 3.5: Definición de la trayectoria de transferencia en el sistema de referencia del robot. 3.6 Definición de trayectorias de transferencia de las patas 41 R2 (F ) = VCH (P1x − Fx)/V2 (3.11) Fase 3: En esta fase el pie desciende verticalmente respecto al sistema de referencia externo hasta que, eventualmente, se detecta el contacto con el terreno. El descenso se prolonga como máximo una distancia H3, superior al doble de la incertidumbre de altura del terreno. En el sistema de referencia del robot el pie se mueve con velocidad vertical V3, mientras que su velocidad horizontal vendrá dada por el planificador de movimiento del cuerpo (véanse la Figura 3.5 y la Figura 3.6). Se define R3 como la distancia recorrida por el cuerpo durante la fase completa, es decir, suponiendo que el pie no choca contra el suelo: R3 = VCHH3/V3 (3.12) Si el pie llegase al ĺımite de su volumen de trabajo, se detendŕıa y finalizaŕıa esta fase. Si al finalizar esta fase no se ha tocado el suelo se considera que la transferencia no ha tenido éxito, y diremos que se ha ejecutado una trayectoria de transferencia completa. 3.6.2. Definición de la trayectoria de un tŕıpode en transferencia Las patas siempre ejecutan sus transferencias agrupadas en tŕıpodes y de manera sincronizada. Cuando el planificador de levantamientos lo indica, las tres patas del tŕıpode inician la fase 1 de sus transferencias. Para comenzar simultáneamente la fase 2, una pata se detendrá al final de la fase 1 si alguna de las otras dos no ha completado esa fase. Para llevar a cabo la fase 3 de manera sincronizada sucede lo mismo. Con esto se consigue que la posición relativa de los puntos de apoyo en el terreno no dependa de las diferencias de tiempo empleado en las transferencias de cada pata. Se considera que la transferencia del tŕıpode termina cuando todas las patas del tŕıpode han tocado el terreno. 3.6.3. Definición de magnitudes Una vez definida la trayectoria de transferencia de una pata y de un tŕıpode se pueden definir los siguientes conceptos necesarios para formular el modo de caminar: Definición 3.5: El recorrido de fase 2 de transferencia RT2(PD, PC , PT ) de un tŕıpode en transferencia a una terna de puntos de apoyo PD, PC , PT 42 Generación de modos de caminar libres para robots hexápodos 0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 Fase 1 Fase 2 Fase 3 P 3 P 1 P 2 P 0 P' 3 P' 2 P' 1 P' 0 X Z Apoyo actual Punto calculado por el planificador de apoyos, F Punto del terreno en que se apoyará el pie en el instante de la búsqueda de apoyos F ' H 1 H 3 0 50 100 150 200 250 300 350 400 -100 -80 -60 -40 -20 0 Fase 1 Fase 2 Fase 3P 3 P 1 P 2 P 0 P' 3 P' 2 P' 1 P' 0 X Y Punto del terreno en que se apoyará el pie en el instante de la búsqueda de apoyos F' R 1 R 1 R 2 R 3 ( a ) ( b ) Figura 3.6: Trayectoria de transferencia en el sistema de referencia del robot: (a) vista lateral, (b) vista superior. es la distancia que se desplaza el cuerpo mientras las tres patas completan la fase 2 de sus transferencias: RT2(PD, PC , PT ) = max (R2(PD), R2(PC), R2(PT )) (3.13) Es decir, dado que las tres patas comienzan cada fase de manera sincronizada, el valor de esta magnitud vendrá dado por la pata del tŕıpode que necesite más tiempo para completar su fase 2 de la transferencia. Nótese que RT2(PD, PC , PT ) es la distancia que recorre el cuerpo desde que se hace la búsqueda de apoyos hasta que todos los pies se han situado sobre sus nuevos puntos de apoyo en el terreno. 3.6 Definición de trayectorias de transferencia de las patas 43 Definición 3.6: El recorrido de transferencia R(PD, PC , PT ) de un tŕıpode en transferencia a una terna de puntos de apoyo PD, PC , PT es la distancia que se desplaza el cuerpo mientras se ejecutan las tres trayectorias de transferencia completas: R(PD, PC , PT ) = R1 + RT2 (PD, PC , PT ) + R3 (3.14) Dada la incertidumbre en la altura del terreno, el cuerpo recorrerá en realidad una distancia menor o igual a R(PD, PC , PT ), dependiendo de la altura a la que los tres pies toquen el terreno. Definición 3.7: El recorrido cinemático, C(P ), de un punto P del volumen de trabajo de una pata se define como: C (P ) = ξ −R1 (3.15) donde ξ es la distancia que se podŕıa desplazar P en dirección opuesta a la de avance del veh́ıculo antes de abandonar el volumen de trabajo (véase la Figura 3.4). Nótese que mientras se cumpla C(P ) > 0 una pata apoyada en P podrá ejecutar la fase 1 de la transferencia (durante la cual el cuerpo se desplaza R1) sin salirse del volumen de trabajo. Definición 3.8: El recorrido cinemático de una terna de puntos, C(PD, PC , PT ) es el menor recorrido cinemático de los puntos PD, PC , PT donde cada punto está asociado a una pata de un tŕıpode. Es decir: C (PD, PC , PT ) = min (C(PD), C(PC), C(PT )) (3.16) Definición 3.9: El recorrido cinemático de un tŕıpode es el recorrido cinemático de la terna de puntos en los que se encuentran los pies de ese tŕıpode. Definición 3.10: El recorrido cinemático del robot, C, es el menor recorrido cinemático de los tŕıpodes en estado de apoyo. Por tanto los tŕıpodes en transferencia no se consideran en el cálculo de esta magnitud: C =    min(C(PD), C(PC), C(PT ), C(QD), C(QC), C(QT )) si dos tripodes en apoyo en PD, PC , PT , QD, QC , QT , min(C(PD), C(PC), C(PT )) si un tripode apoyado en PD, PC , PT (3.17) 44 Generación de modos de caminar libres para robots hexápodos 3.7. Planificador de levantamiento de patas Un criterio extensamente empleado en el diseño de modos de caminar es la maximización del recorrido cinemático, es decir, de la capacidad de la máquina para propulsarse en la dirección deseada teniendo en cuenta sus limitaciones cinemáticas. Aśı, es conveniente levantar antes aquellas patas que tienen menor margen cinemático y que por tanto limitan más el avance del veh́ıculo. Otro criterio que es conveniente considerar en la planificación del levantamiento de las patas es la maximización de la estabilidad de la máquina. Los momentos de menor estabilidad se dan durante los periodos de apoyo en un solo tŕıpode, es decir, cuando el otro tŕıpode realiza su transferencia. Por tanto, es importante planificar con precisión el instante en que se produce esta transferencia con el fin de maximizar la estabilidad. Teniendo en cuenta las consideraciones y definiciones anteriores, el planificador de levantamiento actúa cuando los dos tŕıpodes están en apoyo y se rige por las siguientes reglas: Condición de levantamiento 1: El tŕıpode con menor margen cinemático debe iniciar su transferencia cuando L(PD, PC , PT , Re) = 0. Nótese que Re es la distancia t́ıpica que recorre el robot durante una transferencia. De esta manera, si el cuerpo recorre una distancia Re durante la transferencia, la estabilidad durante la misma será máxima (véase la sección 3.3). Condición de levantamiento 2: El tŕıpode con menor margen cinemático debe iniciar su transferencia si su recorrido cinemático se anula, siempre que esto sea posible con un margen de estabilidad estático ME > MEmin. En ambos casos el tŕıpode a transferir es aquel con menor margen cinemático, con el fin de aumentar la capacidad de la máquina para propulsarse. Normalmente será la condición 1 la que decida el inicio de la transferencia, ya que la forma en que se realiza la búsqueda de apoyos evita que se dé la condición 2 (véanse las condiciones de levantamiento 2 y 3 de la sección 3.9). La condición 2 es útil en situaciones excepcionales como la situación inicial o tras un cambio repentino en la deriva. Nótese que si la distancia recorrida por el robot durante la transferencia es distinta de la prevista (si la transferencia dura más de lo previsto, o si cambia la velocidad del robot, por ejemplo) la estabilidad no será óptima, por lo que Re debe determinarse estimando un tiempo t́ıpico de transferencia, y se calcula Re a partir de este dato y de la velocidad del cuerpo. Además, como se indica en la sección 1.9.1, la búsqueda de apoyos se realiza de manera que se garantice que el cuerpo recorra durante las transferencias una distancia menor o igual que Re. 3.8 Movimiento del cuerpo 45 3.8. Movimiento del cuerpo El movimiento del cuerpo está definido en muchos de sus aspectos por los requerimientos del usuario humano o nivel superior de control encargado de guiar al robot. Otros aspectos del movimiento se determinan de manera automática con el fin de adaptarse a un entorno no estructurado. Los parámetros que definen el movimiento del cuerpo y los métodos empleados para determinarlos se describen a continuación: 1. Ángulo de deriva, α: Es el ángulo formado por el eje longitudinal de la máquina y la dirección de avance. Este parámetro se vaŕıa para generar movimientos laterales del cuerpo. El módulo encargado del guiado del robot puede variar este parámetro para controlar la posición del cuerpo de manera que se ajuste a la trayectoria especificada, según se indica en el caṕıtulo 7. 2. Velocidad horizontal del cuerpo, VCH : Es la velocidad a la que se mueve el cuerpo en la dirección dada por el ángulo de deriva α. Se asume que un módulo superior determina una velocidad nominal VCHN para realizar una determinada tarea. Aśı, el cuerpo avanza por defecto con velocidad VCH=VCHN . Sin embargo, el controlador puede detener automáticamente el avance (i.e. asigna VCH = 0) en los siguientes casos: Si ME ≤ MEmin, es decir, si el robot va a perder su estabilidad. Si C ≤ 0, es decir, si algún pie en estado de apoyo va a salirse de su volumen de trabajo. Si los tres pies de un tŕıpode no tocan el terreno al finalizar la tercera fase de su transferencia. En este caso se planifica un nuevo punto de apoyo según lo expuesto en la sección 3.9.3. La forma en que se planifican los instantes de levantamiento de las patas y los nuevos puntos de apoyos evita normalmente que se den las situaciones anteriores. Sin embargo, se han incluido estas reglas para evitar problemas en situaciones como la inicial, o tras un cambio en la velocidad o la deriva. 3. Velocidad lineal vertical del cuerpo, VBV : Esta velocidad vaŕıa automáticamente para lograr la adaptación al perfil del terreno. La altura media de los pies en apoyo, Zpm, se controla de manera que se mantenga en torno a un valor prefijado, Hm. La velocidad vertical vaŕıa según la siguiente 46 Generación de modos de caminar libres para robots hexápodos ley de control proporcional: VBV = Kpvv (Hm − Zpm) (3.18) donde Kpvv es la ganancia del controlador que se sintoniza experimental- mente. La expresión anterior se emplea siempre que no provoque que un pie se salga de los ĺımites verticales de su volumen de trabajo. En caso contrario se asignará VBV = 0. 4. Velocidad angular de cabeceo, ωCC y balanceo, ωCB del cuerpo: Estas velocidades se determinan automáticamente con el fin de mantener el cuerpo horizontal. Su cálculo se realiza en función de los datos de cabeceo ϕ y balanceo θ del cuerpo (proporcionados de un inclinómetro) y de las posiciones de los pies en apoyo, empleando de nuevo una sencilla ley de control proporcional: ωCC = −Kpϕϕ ωCB = −Kpθθ (3.19) donde Kpϕ y Kpθ son las ganancias del controlador. Al igual que en el caso anterior estas expresiones se emplean siempre que esto no provoque que algún pie se salga de su volumen de trabajo. En caso contrario se asignará ωCC = ωCB = 0. 5. Velocidad angular de guiñada del cuerpo, ωCG : Esta velocidad viene dada por un usuario o módulo superior para cambiar la orientación de la máquina. Todas estas velocidades lineales o angulares del cuerpo se componen para calcular las velocidades lineales instantáneas de cada uno de los pies en apoyo. Las velocidades horizontales de los pies en las fases 1 y 3 de la transferencia también se calculan de esta manera. 3.9. Búsqueda de apoyos La búsqueda de nuevos apoyos para un tŕıpode en transferencia se realiza cuando el tŕıpode está levantado, es decir, cuando ya ha ejecutado la fase 1 de su transferencia y los pies se encuentran en el punto P1 de la trayectoria de transferencia (véanse la Figura 3.3, la Figura 3.5 y la Figura 3.6). La búsqueda se basa en proponer tres puntos candidatos (FD, FC , FT ) a los que se moverán 3.9 Búsqueda de apoyos 47 las tres patas del tŕıpode durante la fase 2 de la transferencia. Cada uno de estos tres puntos se corresponde con el punto P2 de la trayectoria de transferencia en la Figura 3.3, la Figura 3.5 y en la Figura 3.6. Una vez propuesta una terna de puntos candidatos, se evalúa con distintos criterios. Este proceso se repite para distintas ternas de candidatos y, finalmente, se selecciona la mejor terna. La estrategia empleada para evaluar los tres puntos candidatos se compone de dos partes. En la primera parte se imponen una serie de condiciones que clasifican a las ternas como válidas o no válidas desde el punto de vista de la estabilidad, de los ĺımites cinemáticos, etc. En una segunda parte se evaluarán las ternas válidas empleando una función de coste que permita optimizar dos criterios distintos (estabilidad y velocidad máxima) y seleccionar la terna más apropiada. 3.9.1. Condiciones para los puntos de apoyo Dados un tŕıpode de apoyo, un tŕıpode en transferencia, un recorrido estimado de transferencia Re, y tres puntos FD, FC , FT candidatos a apoyo para el tŕıpode en transferencia (respecto al sistema de referencia del robot), los puntos serán válidos si cumplen las siguientes seis condiciones: Condición de apoyo 1: Ninguno de los puntos del terreno en los que se apoye el tŕıpode deben ser puntos prohibidos. Para determinar si un punto candidato F determinado es válido, debe calcularse el punto del terreno F ′ en el que se apoyará el pie al final de la transferencia, que en el sistema de referencia del robot vendrá dado por (véanse la Figura 3.5 y la Figura 3.6): F ′ = F +−→a RT2 (FD, FC , FT ) −→a = (cosα, sinα)T (3.20) Por tanto F ′ D, F ′ C , y F ′ T , deben ser puntos permitidos del terreno. Como se indica en la sección 3.6.3, desde que se realiza la búsqueda de apoyos hasta que todas las patas se sitúan sobre sus nuevos puntos de apoyo el cuerpo se desplazará un distancia RT2(FD, FC , FT ). En términos relativos, el punto del terreno en el que se apoyará el pie se desplaza con respecto al robot la misma distancia. Por esta razón, el punto del terreno en que se apoyará el pie se calcula sumando al punto candidato F un desplazamiento RT2(FD, FC , FT ) en la dirección de avance. Condición de apoyo 2: El instante óptimo, desde el punto de vista de la estabilidad, para iniciar la transferencia del tŕıpode de apoyo debe ser anterior al instante en que se anula el recorrido cinemático del tŕıpode de apoyo. Para 48 Generación de modos de caminar libres para robots hexápodos garantizar esto, se asume que durante la futura transferencia del tŕıpode de apoyo el cuerpo recorrerá una distancia Re. Es decir: L (FD, FC , FT , Re) < C(FD, FC , FT )−RT2 (FD, FC , FT ) (3.21) En esta expresión L(FD, FC , FT , Re) es la distancia que debe desplazarse el cuerpo antes de empezar la transferencia para maximizar la estabilidad durante la misma (véase la sección 3.3), que debe ser, en principio, menor que el recorrido cinemático de las patas, C. Mientras las patas van hacia sus nuevos apoyos el cuerpo se desplaza una distancia RT2(FD, FC , FT ), y por tanto el recorrido cinemático del tŕıpode de apoyo disminuye esa cantidad durante la fase 2 de la transferencia. Por esa razón se resta este término en la derecha en la expresión (3.21). Con esta condición se evita que las patas en apoyo se salgan de sus volúmenes de trabajo antes de que llegue el momento óptimo para transferirlas. Condición de apoyo 3: El recorrido cinemático de los nuevos puntos de apoyo debe ser suficiente para que no se anule antes de que el tŕıpode de apoyo complete su siguiente transferencia. Para imponer esta condición se asume que la siguiente transferencia del tŕıpode de apoyo empezará en el momento óptimo desde el punto de vista de la estabilidad y que el recorrido del cuerpo durante la misma será Re. Por tanto, se debe cumplir la siguiente condición: C (FD, FC , FT ) > L (FD, FC , FT , Re) + Re (3.22) En la parte derecha de la expresión L(FD, FC , FT , Re) es la distancia que se desplazará el cuerpo antes de empezar la siguiente transferencia (según se define en la sección 3.3 y debido a la condición de levantamiento 2, sección 3.7). Dado que Re es la distancia que se recorre durante la transferencia, la suma de ambos términos que aparece en el lado derecho de la expresión es la distancia que recorrerá el cuerpo antes de que se termine la siguiente transferencia. Con esta condición se pretende que las patas del tŕıpode en transferencia no se salgan de sus volúmenes de trabajo antes de que el otro tŕıpode haya completado su siguiente transferencia. Condición de apoyo 4: El tŕıpode en transferencia debe estar apoyado (a pesar de la incertidumbre en la altura del terreno) antes de que llegue el instante óptimo desde el punto de vista de la estabilidad para levantar el otro tŕıpode. Asumiendo que el recorrido del cuerpo durante la transferencia sea Re, esta condición se expresa mediante la siguiente relación: 3.9 Búsqueda de apoyos 49 L (FD, FC , FT , Re) > R3 (3.23) Aśı se garantiza que el tŕıpode de apoyo podrá levantarse en el momento adecuado. Si esta condición no se cumpliera, cuando el tŕıpode se apoye el CDG estará situado por delante del punto óptimo (desde el punto de vista de la estabilidad) para levantar el otro tŕıpode, y ya solo cabŕıa la posibilidad de levantarlo en unas condiciones que no son óptimas. Condición de apoyo 5: Asumiendo que la transferencia se inicie en el momento óptimo desde el punto de vista de la estabilidad (según se indica en la condición de apoyo 2, sección 3.7) y que el recorrido del cuerpo durante la misma sea Re, el margen de estabilidad durante la siguiente transferencia del tŕıpode de apoyo debe ser mayor que un mı́nimo predefinido MEmin. Por tanto: MEt (FD, FC , FT , Re) > MEmin (3.24) Condición de apoyo 6: La distancia recorrida por el cuerpo durante la transferencia del tŕıpode no debe sobrepasar el valor Re asumido en las condiciones 2, 3, 4 y 5. Por tanto se debe cumplir la siguiente expresión: R (FD, FC , FT ) < Re (3.25) Nótese que para formular las condiciones de apoyo 2, 3, 4 y 5 se ha supuesto que durante las transferencias futuras el cuerpo se desplazará una distancia Re. Esto es algo que en principio podŕıa no suceder. Pero por otra parte la condición de apoyo 6 impone que durante la transferencia actual el cuerpo no se desplace más que Re. Por tanto, todas las transferencias, incluidas las futuras cuando llegue el momento de calcularlas, cumplirán esta condición. Y debido a esto, la suposición que se hizo en las condiciones de apoyo 2, 3, 4 y 5 resulta coherente. 3.9.2. Criterios de optimización de los puntos de apoyo Como se avanzó en la sección 3.1 los parámetros a optimizar en el modo de caminar son la estabilidad y la velocidad máxima de la máquina. Para lograr esta optimización se evalúan los candidatos a tŕıpode de apoyo que cumplan las condiciones expuestas en la sección 3.9.1 según la siguiente función de coste: Ev(FD, FC , FT ) = C(FD, FC , FT ) + kevMEt(FD, FC , FT , Re) (3.26) 50 Generación de modos de caminar libres para robots hexápodos donde kev es una constante empleada para dar un peso a la optimización de la estabilidad. De esta manera se trata de premiar de manera lineal la maximización de ambos parámetros, velocidad y estabilidad. 3.9.3. Algoritmo de búsqueda de los puntos de apoyo Para buscar nuevos puntos de apoyo para un tŕıpode en transferencia se definen un número N de posibles apoyos contenidos dentro de cada volumen de trabajo de las patas del tŕıpode. En la Figura 3.7 se pueden ver los N = 15 puntos candidatos empleados para cada pata del robot SILO6. A continuación se analizan todos los posibles tŕıpodes de apoyo formados por las distintas combinaciones de los posibles apoyos de cada pata, comprobando si cumplen todas las condiciones especificadas en la sección 3.9.1 y evaluando aquellos que son válidos según lo expuesto en la sección 3.9.2 (véase la Figura 3.8). Si existe más de una terna de puntos válidos, aquel que obtenga una mayor valoración será el nuevo tŕıpode de apoyo y se procederá a completar la transferencia. Por último, si no se logra tocar el terreno con todos los pies, se definirá una zona prohibida sobre el punto del terreno que no se ha logrado tocar y se procederá a buscar nuevos puntos de apoyo. El diagrama de flujo del algoritmo que describe este proceso se presenta en la Figura 3.8. Adicionalmente, en la Figura 3.9 se analiza el funcionamiento del algoritmo de generación del Candidatos a apoyo Volumen de trabajo Dirección de avance Figura 3.7: Candidatos a puntos de apoyo empleados en la planificación de apoyos del robot SILO6. 3.10 Modos giratorios 51 modo de caminar a lo largo de medio ciclo t́ıpico de locomoción, con el fin de ilustrar la utilidad de las distintas reglas que lo controlan. En esta figura se muestran esquematizados los instantes sucesivos de la locomoción, y se detallan las acciones que se realizan en cada momento, las decisiones efectuadas por el controlador y los efectos que tienen las distintas decisiones en la estabilidad y los márgenes cinemáticos. 3.10. Modos giratorios El modo de caminar descrito en las secciones anteriores se puede extender a un modo giratorio en el que se produce una rotación del robot alrededor de un punto cualquiera (VBHN 6= 0 y ωC 6= 0). Este modo se ha definido como modo circular (véase la sección 2.6). Como caso especial, el robot puede girar sobre su centro de gravedad (por tanto VBHN = 0 y ωC 6= 0), algo útil para producir rotaciones puras (modos rotatorios). En esta sección se describirán el modo circular (giro de radio no nulo) y el modo rotatorio (giro de radio nulo). Este último modo se empleará conjuntamente con el modo de deriva para lograr el seguimiento preciso de trayectorias, según se explica en el caṕıtulo 7. 3.10.1. Modo circular Para transformar el modo de deriva en un modo de giro de radio no nulo únicamente es necesario adaptar aquellas definiciones relacionadas con el desplazamiento del robot y con los movimientos de sus pies. En un modo circular los ángulos girados por el cuerpo y por los pies en apoyo son iguales, mientras que las distancias recorridas por cada pie y por el cuerpo son, en general, diferentes. Por lo tanto, resulta conveniente redefinir en forma angular aquellas magnitudes que representan desplazamientos de un pie o del cuerpo. Para formular el modo circular debeŕıan tenerse en cuenta las siguientes consideraciones: 1. Se considerará que durante las fases 1, 2 y 3 de la transferencia, el robot gira unos ciertos ángulos ρ1, ρ2 y ρ3, equivalentes a las distancias R1, R2 y R3 del modo de deriva y que se pueden calcular de forma similar. 2. El recorrido cinemático angular de un punto P del volumen de trabajo de una pata se define como: CG (P ) = ψ − ρ1 (3.27) 52 Generación de modos de caminar libres para robots hexápodos Determinar una terna de puntos FDi, FCj, FTk candidatos a apoyo trípode en transferencia Cumple la terna FDi, FCj, FTk las condiciones de apoyo 1 a 6? Evaluar la terna FDi, FCj, FTk Definir N posibles apoyos para las patas D, C y T en transferencia: FD1…FDN, FC1…FCN, FT1…FTN. Definir recorrido estimado de transferencia de trípode, Re INICIO Evaluadas todas las ternas candidatas? Si No Si No Transferir el trípode a la terna con mejor evaluación: FDOpt, FCOpt, FTOp Se han apoyado correctamente las tres patas? No Si FIN Definir zonas prohibidas sobre los puntos del terreno en los que las patas no se han logrado apoyar Figura 3.8: Algoritmo de búsqueda de apoyos. 3.10 Modos giratorios 53 L( F ) D C T e ,F ,F ,R P P Q D DF D CF C F T C Q T Q P T Q D P D P C P T C Q T Q R e C D G T (Q ,Q ,Q ,R D C T e) I( ) Q ,Q ,Q ,R D C T e Tr íp od e 1 T Q Q D P D P C P T C Q T QQ D P D P C P T C Q T QQ D P D P C P T C Q a) e) d) c) b) C S (F ) D C T e ,F ,F ,R S (F ) D C T e ,F ,F ,R C (F ) S ( ) Q ,Q ,Q ,R D C T e R e C S ( ) Q ,Q ,Q ,R D C T e Tr íp od e 2 E >S ( ) Q ,Q ,Q ,R D C T e C =0 E =S >E m in (P ) D C T e ,P ,P ,R T rí po de 1 : T rí po de 2 : A va nc e: - Pl an if ic ac ió n:In ic io d e la f as e 1 de la tr an sf er en ci a Pr op ul sa e l c ue rp o L (Q ,Q ,Q ,R ) = 0, e s de ci r, el C O G y I ( co in ci de n. Se c um pl e la c on di ci ón d e le va nt am ie nt o 2 pa ra e l t rí po de 1 D C T e Q ,Q ,Q ,R ) D C T e T rí po de 1 : T rí po de 2 : A va nc e: Pl an if ic ac ió n:In ic io d e la f as e 2 de la tr an sf er en ci a Pr op ul sa e l c ue rp o R 1 de sd e a) C ál cu lo d e nu ev os a po yo s F , F y F pa ra e l t rí po de 1 , a pl ic an do l as co nd ic io ne s de a po yo 1 -6 : 1) F ', F ' y F' so n pu nt os p er m iti do s 2) L (F ,F ,F ,R ) < C -R ( F ,F ,F ) 3) L (F ,F ,F ,R )> R 4) C (F )> L (F ,F ,F ,R e) +R 5) S (F ,F ,F )> E 6) R (F ,F ,F )< R D C T D C T D C T e 2 D C T D C T e 3 D C T e D C T m in D C T e T rí po de 1 : T rí po de 2 : A va nc e: Fi n de la tr an sf er en ci a Pr op ul sa e l c ue rp o Pl an if ic ac ió n: E l t rí po de 1 s e ha a po ya do an te s de l m om en to ó pt im o pa ra le va nt ar el tr íp od e 2 gr ac ia s a la c on di ci ón d e ap oy o 4 ap lic ad a en b ). L a tr an sf er en ci a se h a co m pl et ad o an te s de q ue e l C O G ll eg ue a l p un to T (Q ,Q ,Q ,R ) gr ac ia s a la c on di ci ón d e ap oy o 6, y p or ta nt o la e st ab ili da d du ra nt e la tr an sf er en ci a es m ay or q ue S Q ,Q ,Q ,R la p re vi st a en a ) . D C T e D C T e R d es de c ), R de sd e a) 3 e ( ), e s de ci r, T rí po de 1 : T rí po de 2 : A va nc e: Pl an if ic ac ió n:In ic io d e la f as e 3 de la tr an sf er en ci a Pr op ul sa e l c ue rp o E n el tr íp od e 1 co in ci de n la s po si ci on es d e lo s pi es ( P , P , P ), lo s pu nt os c al cu la do s po r el p la ni fi ca do r de ap oy os ( F , F , F ) y lo s pu nt os d el te rr en o do nd e se v an a a po ya r lo s pi es (F ', F ', F ') . D C T D C T D C TR ( F , F , F ) de sd e b) 2 D C T T rí po de 1 : T rí po de 2 : A va nc e: Pl an if ic ac ió n:In ic io d e la f as e 1 de la tr an sf er en ci a Pr op ul sa e l c ue rp o L (P ,P ,P ,R ) = 0, s e cu m pl e la c on di ci ón d e le va nt am ie nt o 2 pa ra e l t rí po de 2 . L as pa ta s de lt rí po de 2 no se ha n sa lid o de su s vo lú m en es de tr ab aj o gr ac ia s a la co nd ic ió n de ap oy o 3 ap lic ad a en b) . E l tr íp od e 2 po dr á co m pl et ar su tr an sf er en ci a si n qu e la s pa ta s de lt rí po de 1 se sa lg an de su s vo lú m en es de tr ab aj o gr ac ia sa la co nd ic ió n de ap oy o 2. L a tr an sf er en ci a de l t rí po de 2 s e re al iz ar á co n es ta bi lid ad m ay or q ue E gr ac ia s a la c on di ci ón d e ap oy o 5. D C T e m in L (F ,F ,F ,R ) de sd e b) D C T e F T F ' T F C F ' C F D F ' D F ' D F ' T F ' C R 2 D C T e ,F ,F ,R (F ) T (F D C T e ,F ,F ,R ) I(F ) D C T e ,F ,F ,R T (Q ,Q ,Q ,R D C T e) C D G T (P D C T e ,P ,P ,R ) I(P ) D C T e ,P ,P ,R C D G R e C en tr o de g ra ve da d de l r ob ot Pi e en tr an sf er en ci a Pi e en a po yo Pr óx im o pu nt o de a po yo Pu nt o de l t er re no d on de a po ya rá e l pi e R ec or ri do d el c ue rp o du ra nt e la tr an sf er en ci a de l t rí po de 1 (a zu l) o de l t rí po de 2 ( ne gr o) c on es ta bi lid ad ó pt im a N ot a: Po r si m pl ic id ad e n el d ib uj o se c on si de ra q ue e l r ec or ri do c in em át ic o de u n tr íp od e es ig ua l a la d is ta nc ia d e la p at a tr as er a a la r ec ta C =0 Figura 3.9: Evolución del modo de caminar a lo largo de medio ciclo de locomoción t́ıpico. 54 Generación de modos de caminar libres para robots hexápodos donde ψ es ángulo que podŕıa girar P alrededor del centro de giro en sentido opuesto al giro del veh́ıculo antes de abandonar el volumen de trabajo. 3. Las definiciones de punto de inicio de transferencia, punto de fin de transferencia y recorrido de inicio de transferencia (véase la sección 3.3 y la Figura 3.2), que relacionan la estabilidad con el desplazamiento del cuerpo, debeŕıan contemplar la nueva trayectoria circular del CDG. Además, las magnitudes que representen desplazamientos del cuerpo (L, Re) debeŕıan redefinirse como recorridos angulares alrededor del centro de giro de la trayectoria. 4. Finalmente, en el cálculo de apoyos se debe tener en cuenta la trayectoria circular que sigue un punto del terreno (visto desde el robot) para calcular el punto en el que se apoyará el pie al final de la transferencia y determinar, aśı, si está prohibido (condición de apoyo 1). El modo de caminar aśı definido se comportaŕıa igual que el modo de deriva para radios de giro suficientemente grandes, pero no funcionaŕıa adecuadamente para radios pequeños, como se explica a continuación. 3.10.2. Modo rotatorio En caso del modo rotatorio (giro de radio nulo) se da una particularidad: el CDG no se desplaza, y por tanto las condiciones de estabilidad no cambian con el movimiento del cuerpo. Por tanto no tiene sentido definir puntos de inicio y fin de transferencia, recorridos de inicio de transferencia, ni recorridos de transferencia. Y consiguientemente, no son adecuadas la condición de levantamiento 1 ni las condiciones de apoyo 2, 3, 4, 5 y 6, por lo que es necesario redefinir completamente las condiciones de levantamiento y apoyo de las patas. En los modos circulares, pero de radio excesivamente pequeño, los desplazamientos del cuerpo no son suficientes para producir cambios apreciables en la estabilidad. Por lo tanto, en estos casos debeŕıan emplearse también métodos similares a los empleados en el modo rotatorio que se presentan a continuación. Para formular el modo rotatorio se propone un método simplificado para decidir el levantamiento de las patas, expresado en una sola condición: Condición de levantamiento de giro: Un tŕıpode comienza su transferencia cuando el otro tŕıpode finaliza su transferencia. En cuanto a la búsqueda de apoyos se imponen las siguientes condiciones: 3.11 Valoración del algoritmo mediante simulación 55 Condición de apoyo de giro 1: Los pies no se pueden apoyar en zonas prohibidas del terreno. Para lograr esto se debe tener en cuenta la trayectoria circular que siguen los puntos del terreno como se ha expuesto anteriormente. Condición de apoyo de giro 2: El margen de estabilidad absoluto, ME, del nuevo triángulo de apoyo debe ser mayor que un mı́nimo predefinido, MEmin. El margen de estabilidad del nuevo tŕıpode será el margen de estabilidad que habrá a lo largo de toda la siguiente transferencia, por tanto es conveniente que sea suficientemente grande. Condición de apoyo de giro 3: El recorrido cinemático angular del nuevo triángulo de apoyo debe ser mayor que un mı́nimo ρe. Condición de apoyo de giro 4: El ángulo que gira el cuerpo durante la transferencia debe ser menor que un máximo ρe. Las dos últimas condiciones conjuntamente garantizan que mientras un tŕıpode realiza su transferencia el tŕıpode en apoyo no se saldrá de sus ĺımites cinemáticos. 3.11. Valoración del algoritmo mediante simulación Para comprobar la efectividad del algoritmo de generación de modo de caminar se han realizado distintas simulaciones, particularizadas para el caso del robot SILO6. Para la búsqueda de apoyos se han empleado 15 puntos candidatos en cada pata. Esto supone que en cada búsqueda de apoyos se evalúan 153 tŕıpodes, es decir 3375 posibilidades. En un ordenador Pentium III, como el que controla el robot SILO6, esto supone un tiempo inferior a 0.07 segundos, que se considera aceptable dado el periodo de muestreo del controlador (0.1 segundos). Aumentar el número de puntos candidatos supone aumentar el tiempo de cálculo de manera cúbica. Para mantener los tiempos de cálculo en valores aceptables se ha optado por emplear distintos subconjuntos de 15 candidatos adecuados a las caracteŕısticas generales del modo de caminar. Aśı, seŕıa distinto el subconjunto empleado con ángulos de deriva en torno a 0o (avance) y en torno a 180o (retroceso). Los parámetros empleados son MEmin = 50 , H1 = −130 mm , H2 = −110 mm H3 = −200 mm, V1 = 100 m/s, V2 = 300 m/s, V3 = 100 m/s. La primera simulación ilustra la capacidad del algoritmo para generar un modo de caminar cuasi-periódico, muy similar a un modo ondulatorio, a pesar de que este comportamiento no se ha programado expĺıcitamente; por el contrario, 56 Generación de modos de caminar libres para robots hexápodos el comportamiento emerge de las reglas que definen el modo libre cuando la situación es la apropiada. En la Figura 3.10 se observan los resultados obtenidos en esta simulación. Se representa el diagrama de tiempos y el margen de estabilidad absoluto. En el diagrama de tiempos (véase la Figura 3.10(a)) se comprueba que el modo de caminar se asemeja mucho a un modo ondulatorio produciéndose las transferencias de los tŕıpodes a una determinada frecuencia. En cuanto al margen de estabilidad absoluto (véase la Figura 3.10(b)), hay que resaltar el cambio brusco que se produce cuando pasa de la fase de apoyo a la fase de transferencia de un tŕıpode y al contrario. En primer lugar, el margen de estabilidad absoluto disminuye bruscamente al pasar de seis patas en apoyo a tres. En segundo lugar, el margen de estabilidad absoluto crece al pasar de tres patas en apoyo a seis. Al comparar la Figura 3.10(a) y la Figura 3.10(b) se aprecian mejor los instantes en los que el margen de estabilidad absoluto vaŕıa debido a los cambios del estado de las patas. La segunda simulación muestra la capacidad del algoritmo para maximizar la velocidad máxima y la estabilidad. Además, muestra la posibilidad de dar más peso a una de las dos propiedades variando únicamente la constante kev. El aumento de la constante kev se traduce en dar un mayor peso en la función de coste, Ev, al margen de estabilidad absoluto frente al margen cinemático. La Figura 3.11 representa los resultados obtenidos en esta simulación para los siguientes valores de kev: 0, 0.02, 0.04, 0.06, 0.08, 0.1. Los cambios de valores de kev se producen cada 30 segundos. Este cambio se traduce en que las patas se apoyan en puntos de apoyo que proporcionan más estabilidad. Como consecuencia de estos nuevos puntos de apoyo, las fases de apoyo, y por tanto, las fases de transferencia también, tienen una menor duración. La disminución de la duración de las fases de apoyo y transferencia, y el aumento del margen de estabilidad al aumentar kev se comprueba en la Figura 3.11(a) y la Figura 3.11(b), respectivamente. Por otra parte, se puede observar un aumento en el valor mı́nimo del margen cinemático (véase la Figura 3.11(c)). Esto se debe a que al durar menos las fases de apoyo el levantamiento de las patas se produce antes de que el recorrido cinemático se anule. Hay que resaltar que a pesar del aumento monótono creciente de la constante kev no se produce una evolución continua en el cambio del margen de estabilidad absoluto y el margen cinemático. Esto se debe a la discretización del espacio en el proceso de búsqueda de apoyos. Como se presentó en la sección 3.9, para realizar la búsqueda de apoyos se toman 15 posibles puntos de apoyo para cada pata 3.11 Valoración del algoritmo mediante simulación 57 0 10 20 30 40 50 60 70 80 90 100 1 2 3 4 5 6 (a) Tiempo (s) E st ad os d e la s pa ta s 0 10 20 30 40 50 60 70 80 90 100 150 200 250 300 350 400 450 (b) Tiempo (s) M ar ge n de e st ab ili da d Figura 3.10: (a) Diagrama de tiempos: pata en apoyo (ĺınea continua), pata en transferencia (ĺınea discontinua); (b) Margen de estabilidad absoluto. del tŕıpode en transferencia. La discretización de estas posibilidades supone que la variación de los márgenes no sea continua, ya que el cambio de un punto de apoyo a otro en la búsqueda de apoyos no es continuo respecto al aumento de la constante kev. Una simulación parecida a la anterior es la que se presenta en la Figura 3.12. En este caso la constante kev se mantiene en el valor 0.01 y la velocidad toma los siguientes valores: 25 mm/s, 30 mm/s, 35 mm/s, 40 mm/s, 45 mm/s, 50 mm/s, 55 mm/s, 60 mm/s, 65 mm/s, 70 mm/s, 75 mm/s, 80 mm/s y 85 mm/s. Los cambios en la velocidad se producen cada 30 segundos. El comportamiento del modo de caminar es similar al observado en la simulación anterior, como se puede apreciar en el diagrama de tiempos, ya que al aumentar la velocidad del robot las fases de apoyo y de transferencia duran menos (véase la Figura 3.12(a)). En el 58 Generación de modos de caminar libres para robots hexápodos 0 50 100 150 200 250 0 2 4 6 (a) Tiempo (s) E st ad os d e la s pa ta s 0 50 100 150 200 250 100 200 300 400 500 (b) Tiempo (s) M ar ge n de e st ab ili da d 0 50 100 150 200 250 2 4 6 8 10 (c) Tiempo (s) M ar ge n de c in em at ic o Figura 3.11: Modo de caminar al aumentar la constante kev:(a) Diagrama de tiempos: pata en apoyo (ĺınea continua), pata en transferencia (ĺınea discontinua); (b) Margen de estabilidad absoluto; (c) Margen cinemático. caso del margen de estabilidad absoluto no sucede lo mismo. Cuando la velocidad es inferior a 70 mm/s los valores entre los que fluctúa esta variable se mantienen constantes y únicamente vaŕıa la frecuencia de las transferencias al aumentar la velocidad (véase la Figura 3.12(b)). En el caso del margen cinemático se aprecia una relación más directa con la velocidad. En la Figura 3.12(c) se observa que tanto los valores máximos como los mı́nimos alcanzados por esta magnitud a lo largo de un ciclo disminuyen progresivamente. Esto se debe que a a mayor velocidad el cuerpo recorre una distancia mayor durante cada transferencia y, por tanto, los puntos de apoyo deben cambiar a otros con menor margen cinemático para seguir garantizando la estabilidad. Esto hace a su vez que la frecuencia de las transferencias aumente y el recorrido de las mismas disminuya. También hay que resaltar que a partir de una velocidad superior a 80 mm/s ambos márgenes 3.11 Valoración del algoritmo mediante simulación 59 0 50 100 150 200 250 300 350 400 0 2 4 6 (a) Tiempo (s) E st ad os d e la s pa ta s 0 50 100 150 200 250 300 350 400 0 200 400 600 (b) Tiempo (s) M ar ge n de e st ab ili da d 0 50 100 150 200 250 300 350 400 0 5 10 (c) Tiempo (s) M ar ge n ci ne m at ic o Figura 3.12: Modo de caminar al aumentar la velocidad del robot:(a) Diagrama de tiempos: pata en apoyo (ĺınea continua), pata en transferencia (ĺınea discontinua); (b) Margen de estabilidad absoluto; (c) Margen cinemático. alcanzan el valor nulo. Esto significa que el modo de caminar comienza a fallar ya que no asegura la estabilidad del robot (véase la Figura 3.12). La tercera simulación muestra la capacidad del algoritmo para caminar por un terreno con zonas prohibidas. En esta simulación se generan aleatoriamente zonas prohibidas en el terreno y el robot se desplaza en ĺınea recta sobre ellas. Las zonas prohibidas se representan en negro. En gris se representa un área de seguridad para los apoyos de los pies. Se admite que el centro del pie del robot pise sobre el ĺımite de estas zonas de seguridad cercanas a las zonas prohibidas, pero no dentro. De esta forma se asegura que el pie no se apoya sobre una zona prohibida. En la Figura 3.13(a) se presenta el comportamiento del robot sin tener en cuenta las zonas prohibidas sobre el terreno. Se observa cómo la posición de los puntos de apoyo de las patas caen sobre una zona prohibida en algunas casos. 60 Generación de modos de caminar libres para robots hexápodos Figura 3.13: Modo de caminar sobre terreno con zonas prohibidas. Puntos de apoyo de patas delanteras (ćırculo azul), puntos de apoyo de patas centrales (ćırculo rojo) y puntos de apoyo de patas traseras (ćırculo negro). (a) Sin tener en cuenta zonas prohibidas; (b) Teniendo en cuenta zonas prohibidas. El correcto funcionamiento del modo de caminar cuando se tienen en cuenta las zonas prohibidas se comprueba en la Figura 3.13(b). En este caso se observa cómo el robot va evitando las zonas prohibidas y ninguna pata cae sobre una de estas zonas. El cuarto caso de simulación muestra la capacidad del algoritmo para adaptarse a los cambios en la trayectoria, propiedad útil para lograr el seguimiento preciso de trayectorias. Esta simulación consiste en producir un cambio brusco del ángulo de deriva, de 0o a 20o, y observar el comportamiento del modo de caminar. 3.11 Valoración del algoritmo mediante simulación 61 −800 −600 −400 −200 0 200 400 600 800 −800 −600 −400 −200 0 200 400 600 800 X (mm) Y ( m m ) Figura 3.14: Modo de deriva. Fase 1 (deriva 0o): todas las patas en fase de apoyo (azul). Para comprobar el correcto funcionamiento del modo de deriva se representan las posiciones de cada pata en su volumen de trabajo correspondiente a lo largo de medio ciclo de locomoción y respecto al sistema de referencia del robot. Téngase en cuenta que en la Figura 3.14, la Figura 3.15, la Figura 3.16, la Figura 3.17 y la Figura 3.18 cada figura está englobada en la siguiente. En la primera fase se representa las posiciones de cada pata cuando el ángulo de deriva es 0o y todas las patas están en la fase de apoyo. El robot avanza en el sentido positivo y paralelo al eje x y las trayectorias de las patas en apoyo son rectas y en sentido negativo al eje x (véase la Figura 3.14). Con ĺınea discontinua se representan los volúmenes de trabajo. En la segunda fase tiene lugar la transferencia del tŕıpode 2 y durante esta fase 62 Generación de modos de caminar libres para robots hexápodos −800 −600 −400 −200 0 200 400 600 800 −800 −600 −400 −200 0 200 400 600 800 X (mm) Y ( m m ) Figura 3.15: Modo de deriva. Fase 2 (deriva 0o): patas 1, 4, y 5 en fase de apoyo (azul), patas 2, 3 y 6 en transferencia (rojo). se producirá el cambio del ángulo de deriva a 20o. Se representan las posiciones de las patas en fase de apoyo (patas 1, 4 y 5) y las posiciones de las patas en transferencia (patas 2, 3 y 6) hasta el instante en que se produce el cambio en el ángulo de deriva. Se observa que las trayectorias de las patas siguen siendo rectas y paralelas al eje x. Las trayectorias de las patas en transferencia se realizan en el sentido positivo del eje x, el mismo sentido de avance del cuerpo (véase la Figura 3.15). En la tercera fase tiene lugar, durante la transferencia del tŕıpode 2, el cambio del ángulo de deriva de 0o a 20o. Se representan las posiciones de las patas en fase de apoyo (patas 1, 4 y 5) y las posiciones de las patas en transferencia (patas 3.11 Valoración del algoritmo mediante simulación 63 −800 −600 −400 −200 0 200 400 600 800 −800 −600 −400 −200 0 200 400 600 800 X (mm) Y ( m m ) Figura 3.16: Modo de deriva. Fase 3 (deriva 20o): patas 1, 4, y 5 en fase de apoyo (verde), patas 2, 3 y 6 en transferencia (amarillo). 2, 3 y 6) a partir del instante en el que se ha producido el cambio del ángulo de deriva y su valor es 20o. Se observa el cambio en la trayectoria de las patas en apoyo para adaptar la dirección de avance del cuerpo a un ángulo de 20o respecto al eje x (véase la Figura 3.16). En la cuarta fase se representa las posiciones de cada pata cuando la deriva es 20o y todas las patas están en la fase de apoyo. El robot avanza en el sentido positivo del eje x y con un ángulo de 20o respecto al eje x, lo que se aprecia en los movimientos de los pies en apoyo (véase la Figura 3.17). En la quinta fase tiene lugar la transferencia del tŕıpode 1. Se representan las posiciones de las patas en fase de apoyo (patas 2, 3 y 6) y las posiciones de las 64 Generación de modos de caminar libres para robots hexápodos −800 −600 −400 −200 0 200 400 600 800 −800 −600 −400 −200 0 200 400 600 800 X (mm) Y ( m m ) Figura 3.17: Modo de deriva. Fase 4 (deriva 20o): todas las patas en fase de apoyo (verde). patas en transferencia (patas 1, 4 y 5). Las trayectorias de las patas en apoyo continuan en la misma dirección que en la cuarta fase y las patas en transferencia se adaptan a la deriva. Esto se comprueba al comparar las transferencias que tienen lugar en la fase 2 y 3 cuando es el tŕıpode 2 el que se transfiere. El modo de deriva funciona correctamente en la simulación realizada y se observa el comportamiento de las patas del robot para adaptar el cuerpo del robot al cambio del ángulo de deriva que tiene lugar. 3.12 Valoración del algoritmo experimentalmente 65 −800 −600 −400 −200 0 200 400 600 800 −800 −600 −400 −200 0 200 400 600 800 X (mm) Y ( m m ) Figura 3.18: Modo de deriva. Fase 5 (deriva 20o): patas 2, 3 y 6 en fase de apoyo (verde), patas 1, 4, y 5 en transferencia (amarillo). 3.12. Valoración del algoritmo experimentalmente Se ha llevado a cabo un experimento para comprobar el correcto funcionamiento cuando el robot se desplaza sobre un terreno con presencia de zonas prohibidas. Este experimento es similar a la tercera simulación descrita en la sección 3.11. La posición de las zonas prohibidas se toman con un sistema de medición preciso, DGPS, de manera que se conoce su localización espacial antes de iniciar el movimiento del robot. Las zonas prohibidas se representan por ćırculos de color rojo de 0.05 m de diámetro que son equivalentes a los cuadrados de 0.05 m de lado descritos en la tercera simulación de la sección 3.11. También se considera un 66 Generación de modos de caminar libres para robots hexápodos área de seguridad de color negro de 0.15 m de diámtero para cada zona prohibida que es equivalente al cuadrado gris de la tercera simulación en la sección 3.11 (véase la Figura 3.19). Zona prohibida Área de seguridad Figura 3.19: Terreno en el que se desplaza el robot con presencia de zonas prohibidas (color rojo) y un área de seguridad (color negro). El robot evita perfectamente las zonas prohibidas durante su desplazamiento. Esto se muestra en una secuencia del movimiento presentada en la Figura 3.20. Se observa una transferencia de un tŕıpode y cómo los puntos de apoyo de los pies no caen sobre alguna de las zonas prohibidas existentes. La forma más sencilla de presentar los resultados de este experimento es mediante un video donde se muestra cómo el robot se va desplazando por un terreno en presencia de zonas prohibidas y éste evita pisarlas (véase http://www.iai.csic.es/users/SILO6/SILO6 pics/forbiddenareas.wmv). Esto es un experimento preliminar ya que en situaciones reales no se tendrá conocimiento de la posición de las zonas prohibidas. Estas posiciones serán reportadas por el manipulador en el instante en que se detecta una posible mina (zona prohibida) con el detector de metales. 3.12 Valoración del algoritmo experimentalmente 67 Figura 3.20: Transferencia de un tŕıpode evitando pisar sobre zonas prohibidas (secuencia de izquierda a derecha y de arriba a abajo.) 68 Generación de modos de caminar libres para robots hexápodos Caṕıtulo 4 Técnicas de localización en exteriores 4.1. Introducción La robótica móvil es un área de investigación que tiene como último objetivo el desarrollo de robots autónomos que puedan desenvolverse en ambientes dinámicos. Se presentan tres problemas para alcanzar este objetivo: la navegación, la localización, y el reconocimiento del entorno. La localización es un problema clave para proporcionar capacidades autónomas a un robot móvil. La localización del robot implica el conocimiento en cada momento de su posición y orientación (pose) respecto a una representación global del espacio. En este caṕıtulo se presenta el problema de la localización de robots, fundamentalmente en exteriores, y se desarrolla un estudio bibliográfico sobre las diferentes técnicas utilizadas para llevar a cabo la tarea de localización (dead- reckoning, fusión sensorial, localización multi-robot,...). 4.2. El problema de la localización El problema de la localización de robots consiste simplemente en responder a la pregunta: ¿dónde estás?; por lo tanto el robot tiene que conocer su posición, al menos relativamente, en el entorno en que se encuentre. Cuando hablamos 70 Técnicas de localización en exteriores z y x z! y! x! W PB x!x orientación W B W Pj B Pj Θ Figura 4.1: Sistema de referencia del robot móvil. de localización o posición, pose1, queremos decir las coordenadas x, y y la orientación, α, del sistema de referencia solidario al robot en un sistema de referencia absoluto o fijo. Este problema de localización es muy importante en robots autónomos ya que si el robot no sabe dónde está, dificilmente podrá llegar a un destino. Para muchos autores, éste es el problema fundamental para proporcionar capacidades autónomas reales a los robots. El conocimiento de la localización del robot (posición y orientación) con respecto a un sistema de referencia absoluto permite generar trayectorias, evitar obstáculos, obtener mapas del terreno, controlar el ángulo de deriva y orientación del robot, etc. La localización de un robot móvil queda definida por las componentes de translación (tx, ty, tz) y de rotación (θx, θy, θz) del sistema de coordenadas solidario al robot (x′, y′, z′) con respecto a un sistema de referencia absoluto (x, y, z). Normalmente, aunque depende de las aplicaciones, se considera el caso bidimensional, es decir, el robot se proyecta verticalmente sobre un plano XY nivelado para calcular su localización. De esta forma hay que determinar su posición (tx, ty) y su orientación (θ) (véase la Figura 4.1). 1En español significa: (1) posición o postura inmóvil o poco natural; (2) comportamiento, actitud o modo de expresarse estudiado o fingido. En inglés hace referencia a la posición y orientación del robot. En esta tesis se utiliza en el sentido del término inglés. 4.2 El problema de la localización 71 4.2.1. Caracteŕısticas de la localización Se pueden emplear diferentes técnicas para la localización de robots móviles que dependen de aspectos tales como: 1. El tipo de entorno en el cual el robot móvil se desplaza 2. El conocimiento que se tiene del entorno 3. La tarea a realizar 4. El sistema sensorial utilizado 4.2.1.1. Entorno de la tarea La dependencia del entorno distingue la tarea de localización entre: localización en interiores y localización en exteriores. La localización en interiores se lleva a cabo en entornos estructurados (interiores de edificios en los que pueden detectarse y distinguirse las paredes, esquinas, obstáculos fijos, medir distancias entre las paredes, etc.); y que pueden ser modelados. Por otro lado están los obstáculos móviles como pueden ser personas u otros robots, que no se modelan fácilmente. Un ejemplo de localización en interiores es la localización de un robot gúıa en un museo (Fox et al., 1999b; Dellaert et al., 1999) donde la posición de puertas, paredes, y objetos fijos son conocidos a priori. En cambio la localización en exteriores tiene lugar en entornos naturales que no son estructurados y por lo tanto dif́ıciles de modelar (Cobano et al., 2007). Tareas humanitarias de desactivación de minas antipersonas, exploración de volcanes y tareas forestales, son claros ejemplos de actividades en exteriores donde los obstáculos y las caracteŕısticas del terreno no son conocidas a priori. Por esta razón, los sensores requeridos en uno u otro entorno son muy diferentes y resultan ser exclusivos de un entorno. Como ejemplo se puede mencionar el sistema GPS (Global Positioning System)2 o Sistema de Posicionamiento Global, que no se puede utilizar para la localización de robots móviles en entornos interiores debido a la imposibilidad de recibir la señal de los satélites. Otro ejemplo de la dependencia del entorno son los sensores ultrasónicos que no pueden utilizarse en entornos naturales ya que se ven afectados por la lluvia y el viento, además de presentar un rango de distancias corto para lo que se requiere en entornos naturales. 2Aunque GPS lleva impĺıcito el término sistema, lo referiremos como el sistema GPS 72 Técnicas de localización en exteriores 4.2.1.2. Conocimiento sobre el entorno La diferencia más importante entre localización en interiores y exteriores es el tipo de conocimiento que se tienen de la posición a priori. En general, la información a priori proporcionada al robot describe el entorno en el cual se está moviendo el robot. Esta información especifica caracteŕısticas que son invariantes en el tiempo y, por lo tanto, se pueden usar para determinar la posición del robot. El ejemplo más claro es la utilización de mapas. En interiores se pueden crear mapas del entorno a partir de la información obtenida con los sensores. Estos mapas pueden jugar un papel muy importante para localizar el robot ya que permiten comparar los datos obtenidos con los sensores que nos dan información sobre la percepción del entorno con los datos que nos proporcionan los mapas. De este modo se puede obtener un conocimiento a priori de la posición del robot. En el caso de la localización en exteriores, en general, no se puede cotejar la información obtenida con los sensores con mapas del entorno ya que suelen ser entornos naturales desconocidos y dinámicos (cambiantes). De esta forma se puede distinguir entre la localización basada en la percepción del entorno (Thurn et al., 2001) y la que no se basa en la percepción del entorno, también conocida como localización expĺıcita o estimadores expĺıcitos (Cobano et al., 2004; Kuroda et al., 2004). En el primer caso, como se ha mencionado en el párrafo anterior para estimar la pose del robot móvil, se utilizan los sensores que dan información sobre el entorno y, comparando esta información con los datos o modelos conocidos (mapas), infieren la localización del robot móvil (estimación mediante la percepción del entorno). En el segundo grupo se obtiene la localización del robot móvil a partir de medidas obtenidas de los sensores sin realizar una interpretación del entorno (estimación expĺıcita de la posición). Este último caso se tratará en esta tesis doctoral. 4.2.1.3. Tipo de tarea El tipo de localización del robot depende de la tarea espećıfica a realizar. Hay tareas donde el seguimiento de la trayectoria del robot es el requisito principal, otras tareas, por ejemplo, requieren precisión en el objetivo final. Esta tesis doctoral está motivada por una aplicación concreta que es la detección y localización de minas antipersonas en tareas de desactivación humanitaria, la cual requiere una estimación exacta de la posición del robot en entornos naturales. Estos entornos son desconocidos a priori, no existen mapas 4.2 El problema de la localización 73 sobre ellos, y por lo tanto, el sistema sensorial no se puede basar en la percepción del entorno. 4.2.1.4. Sistema sensorial Para localizar de un robot, se pueden emplear diferentes sensores situados en las articulaciones de las patas del robot (robots caminantes) o en el eje de las ruedas del robot (robots con ruedas). Estos sensores (encoders, resolvers, potenciómetros, acelerómetros, tacómetros), normalmente, proporcionan información incremental. A través de esta información podemos calcular la posición. Esta posición se calcula respecto a la posición inicial mediante la acumulación de los incrementos obtenidos en cada instante, por lo tanto ésta será una posición relativa respecto a la posición inicial. La posición absoluta del robot respecto a un sistema de referencia externo se puede calcular a partir de técnicas basadas en la transmisión de señales de radio usando una estación base y otra remota. La técnica más usada es el Sistema de Posicionamiento Global (GPS, Global Positioning Systems). Otras técnicas emplean marcas sobre el terreno (landmarks) para calcular la posición del robot mediante métodos de triangulación. La orientación del robot se puede obtener mediante diferentes tipos de sensores como giróscopos, brújulas electromagnéticas, etc. En todas las técnicas se produce una incertidumbre o error en la localización (estimación de la posición y orientación). Esta incertidumbre tiene un ĺımite que depende del entorno, la naturaleza de la tarea a realizar y la precisión requerida en los movimientos. La precisión depende de la técnica y el sistema sensorial utilizados. 4.2.2. Casos t́ıpicos de la localización En la localización general de robots pueden presentarse los siguientes problemas. 1. El problema de seguimiento de la posición en el que el robot conoce su posición inicial. En este caso se trata de mantener una trayectoria determinada mientras el robot está navegando en un entorno. Las técnicas utilizadas en este caso se denominan técnicas locales. 2. El problema de posicionamiento global. Éste es más dif́ıcil que el anterior porque el robot no conoce su posición inicial. Los métodos que 74 Técnicas de localización en exteriores solucionan este problema se denominan técnicas globales. 3. El problema del robot “raptado”. Éste se da cuando el robot conoce su posición y de pronto es llevado a otra posición sin que el robot detecte el cambio súbito de posición. El problema al que se enfrenta el robot es saber que ha sido ”raptado” y descubrir su nueva posición. 4.2.3. Información disponible por el robot El robot puede tener acceso a dos tipos de informaciones para llevar a cabo la tarea de localización: 1. Información a priori. El robot obtiene este tipo de información recogida por él mismo o por fuentes externas. En este caso, el ejemplo más claro es la información que el robot tiene disponible a través de los mapas del entorno. 2. Información navegacional. El robot consigue información sobre el entorno mediante cada observación y acción que él realiza durante la navegación; este tipo de información se conoce como información navegacional. Durante la navegación el robot realiza dos tipos de acciones. Por una lado, el robot se mueve por medio de un sistema de conducción. Este sistema suele consistir en ruedas, orugas o patas. A estos componentes les denominaremos actuadores. Mediante estos sistemas se cambia la posición del robot, luego juega un papel muy importante para la tarea de localización. A través de estos actuadores se puede estimar el desplazamiento que sufre el robot, estas posiciones estimadas son relativas. Por otro lado el robot percibe el entorno mediante sus sensores. Estas observaciones del entorno proporcionan información sobre la posición del robot independiente de la posición anterior, por lo tanto las medidas son absolutas. 4.2.4. Tipos de localización Se distinguen dos tipos de localización dependiendo del tipo de información que se recoge del sistema sensorial que tiene el robot: 4.3 Localización en exteriores 75 4.2.4.1. Localización relativa Se evalúa la posición y orientación usando información proporcionada por sensores situados a bordo del robot. Mediante estos sensores se obtienen incrementos de posición referidos a la posición anterior. El problema que presenta este tipo de localización es que el error de la posición aumenta con la distancia. 4.2.4.2. Localización absoluta Se obtiene la posición absoluta utilizando sensores exteroceptivos, es decir, aún pudiendo ir a bordo del robot proporcionan la posición y orientación a partir de una referencia exterior. En este caso la localización es independiente de la estimación de la posición previa. Tiene la ventaja de que el error en la estimación de la posición no crece con la distancia recorrida como en el caso de la localización relativa. 4.3. Localización en exteriores La estimación de la pose de un robot se puede realizar a partir de medidas internas o de medidas externas. Las medidas internas se refieren a las realizadas desde los sensores integrados en el robot que informan del estado del robot. Estos sensores proporcionan información sobre los incrementos de posición (encoders), velocidad (tacómetros), aceleración (acelerómetros o giróscopos, etc.). En cambio las medidas externas informan sobre la situación del robot en el entorno. Suelen estar formados por dos partes diferenciadas: por un lado un receptor que generalmente está a bordo del robot, y por otro lado de un emisor situado en el entorno (GPS, balizas, etc.) 4.3.1. Medidas internas La técnica más simple y más utilizada para estimar la posición y orientación de un robot móvil consiste en sumar los incrementos de la trayectoria recorrida por el robot y que éste recoge con una determinada frecuencia de muestreo. Estos incrementos se obtienen a partir de las medidas de sensores internos de velocidad, aceleración, orientación, etc. Dependiendo de los sensores internos que se utilicen existen dos técnicas diferentes: odometŕıa (o sistemas odométricos) y técnicas dead-reckoning (o sistemas de navegación inercial). 76 Técnicas de localización en exteriores 4.3.1.1. Odometŕıa El método por el cual se estima la posición de un robot móvil utilizando sólo sensores de desplazamiento (encoders, potenciómetros, resolvers ...) se denomina odometŕıa. Se basa en la integración de información incremental (Ojeda y Borenstein, 2004). La odometŕıa es el método más utilizado para determinar la posición momentánea de un robot móvil. En la mayoŕıa de las aplicaciones prácticas, la odometŕıa proporciona fácilmente información de la posición en tiempo real. Los errores que se introducen en la odometŕıa son elevados, por lo tanto algunos estudios se han centrado espećıficamente en reducir estos errores (Borenstein y Feng, 1996). En el caso de robots con ruedas, las dos fuentes principales de error son: 1. La imprecisión en la distancia efectiva de ejes. 2. La desigualdad de los diámetros de las ruedas. En el caso de robots caminantes, las fuentes de error principales son: 1. Los deslizamientos en los pies. 2. Las holguras en las articulaciones. 3. Las flexiones de los eslabones y articulaciones de las patas del robot. 4. La calibración cinemática. La odometŕıa en robots caminantes se basa en la suposición de que las medidas de los sensores de desplazamiento (generalmente encoders) situados en las articulaciones de las patas del robot se pueden traducir en medidas del desplazamiento del robot utilizando el modelo cinemático del robot. Ésta es una forma clásica de calcular la posición de un robot móvil con respecto a una posición inicial (localización relativa). Es importante destacar el hecho de que con este método no se obtienen posiciones absolutas sino posiciones relativas. La gran ventaja de la odometŕıa es su simplicidad y bajo coste; su principal problema es la precisión, la cual decrece con la distancia recorrida. 4.3 Localización en exteriores 77 4.3.1.2. Sistemas de navegación inercial: dead-reckoning La técnica dead-reckoning emplea métodos geométricos simples (un modelo cinemático del robot) sobre datos odométricos para computar la posición del robot relativa a su posición inicial (localización relativa) a través de los incrementos de posición respecto a la posición anterior. Se utilizan diferentes tipos de sensores para computar diferentes magnitudes, entre los que encontramos, encoders ópticos incrementales para calcular los incrementos de los desplazamientos, acelerómetros, giróscopos para el cálculo de las orientaciones, que pueden ser mecánicos (masa giratoria), ópticos (de anillo láser o de fibra óptica), o piezoeléctricos, y también se pueden emplear brújulas electromagnéticas. Esta técnica se usa mucho por su simplicidad, bajo coste y robustez. Dead- reckoning se puede definir como la determinación, sin la ayuda de observaciones externas, de la posición y orientación de un robot teniendo en cuenta la trayectoria viajada y la tendencia o deriva. Dead-reckoning presenta problemas en distancias largas debido a varios inconvenientes: 1. El modelo cinemático siempre tiene algunas inexactitudes 2. Hay fuentes externas afectando al movimiento que no son observables por los sensores (deslizamientos, holguras, etc.) y por lo tanto es dif́ıcil medir el desplazamiento real que tenga lugar. Esta técnica se ha aplicado generalmente en barcos y robots con ruedas, pero también se ha introducido en robots caminantes (Roston y Krotkov, 1991). Las técnicas dead-reckoning son más fiables y precisas que la odometŕıa (o los sistemas odométricos) ya que las primeras no se ven afectadas por los problemas derivados de la interacción del robot con el suelo y además pueden corregir los efectos de las ondulaciones e irregularidades del terreno. La desventaja es que las técnicas dead-reckoning son más caras. Ambas técnicas presentan la ventaja de su fácil implementación, pero presentan una gran desventaja para el cálculo de la posición absoluta debido a que los errores se acumulan y crecen proporcionalmente a la distancia recorrida. Por este motivo, estas técnicas no suelen utilizarse aisladamente sino combinadas con un sistema de posicionamiento absoluto para reducir los errores introducidos con el desplazamiento (Roumeliotis y Bekey, 1997). 78 Técnicas de localización en exteriores 4.3.2. Medidas externas Las diferentes técnicas de localización que se pueden emplear con medidas externas se basan fundamentalmente en estaciones de transmisión y en balizas. 4.3.2.1. Estaciones de transmisión El posicionamiento absoluto basado en estaciones de transmisión se utiliza desde hace décadas en aplicaciones maŕıtimas y aeronáuticas. En los últimos años, estos sistemas han sido objeto de estudio para su aplicación sobre robots móviles en exteriores debido a que proporciona una localización absoluta del robot sin requerir un conocimiento del entorno, lo que la hace adecuada para los casos en los que el robot debe recorrer largas distancias o cuando éste opera en entornos muy diversos. Estos sistemas de posicionamiento se componen de un receptor a bordo del robot y un conjunto de estaciones transmisoras de ondas de radio3. Se pueden considerar dos grupos: los que se basan en estaciones fijas y los que se basan en estaciones móviles. Los primeros se emplean en la navegación de barcos, submarinos y aviones. Se caracterizan por el empleo de señales de radio de media y alta frecuencia que son emitidas por estaciones terrestres fijas. Los segundos operan desde satélites y se utilizan en aplicaciones militares y en la navegación civil. Esto ha desembocado en el sistema GPS, mucho más potente, que se está utilizando desde hace algunos años en robots móviles. El sistema GPS está compuesto por una constelación de 24 satélites situados en órbita a una altura de 20.200 Km. de la Tierra y un receptor. Esta red de satélites se denomina NAVSTAR (NAVigation System and Ranging), es propiedad del Gobierno de los Estados Unidos de América y está gestionado por su Departamento de Defensa. El sistema GPS permite determinar la posición en cualquier lugar del planeta, a cualquier hora del d́ıa y bajo cualquier condición meteorológica. Este sistema es otra de las técnicas con la que se obtiene una localización absoluta. Para que un robot pueda computar su localización en un espacio tridimensional, un receptor GPS debe recibir señales de al menos cuatro satélites. Es dif́ıcil mantener la señal de cuatro o más satélites porque cada señal se transmite a 1,57542 GHz que es demasiado elevada para rodear o pasar a través de objetos sólidos en la trayectoria de la señal. Por este motivo, los receptores GPS no se pueden utilizar en interiores. En exteriores también puede haber obstáculos, 3Este tipo de sistemas de posicionamiento se conoce también como Radio-Navegación 4.4 Fusión Sensorial 79 sobre todo en medios urbanos, lo que causa que el receptor GPS no reciba señal durante un tiempo, por esta razón un sistema de posicionamiento de un veh́ıculo o robot no puede posicionarlo continuamente usando sólo un receptor GPS. Aún recibiendo señales de más de cuatro satélites la precisión que se alcanza en algunos casos es de 100 m y este es un error muy alto para aplicaciones de navegación de robots. La exactitud inherente del receptor GPS es mucho mejor, pero la señal de los satélites GPS se degrada intencionadamente por el Departamento de Defensa de los Estados Unidos por motivos de seguridad nacional. Esta degradación se denomina disponibilidad selectiva (SA, Selective Availability). En la actualidad el gobierno de los Estados Unidos no está degradando la señal y por lo tanto la exactitud de las posiciones fijas del GPS ha mejorado significativamente. Una forma más precisa de posicionamiento GPS es la conocida como Sistema de Posicionamiento Global Diferencial (DGPS, Differential Global Positioning System). El receptor GPS recibe correcciones desde una estación fija con lo que se consigue mejorar la precisión considerablemente (Abbott y Powell, 1999). El sistema GPS se presenta con más detalle en el Apéndice B. 4.3.2.2. Estimación mediante balizas Este tipo de sistemas permite determinar la posición del robot en un entorno restringido mediante el emplazamiento en el escenario de navegación de un determinado número de balizas de posición conocida (beacons). La posición se determina por triangulación y no se precisa la percepción del entorno. La precisión y fiabilidad depende fundamentalmente del tipo de señal utilizada (infrarrojos, láser, radio, etc.), de las caracteŕısticas del sensor, y del número de balizas utilizadas en la triangulación. En cuanto a las desventajas, hay que resaltar la necesidad de configurar apropiadamente el entorno de trabajo y garantizar que se puede utilizar un suficiente número de estas señales. Esto impide la aplicación de este sistema de estimación de la posición en nuestra aplicación espećıfica debido a la dificultad que presenta en entornos muy dinámicos o no-estructurados. 4.4. Fusión Sensorial La fusión de datos es un marco formal en el que se expresan los métodos y herramientas que permiten la integración de datos originados por diferentes 80 Técnicas de localización en exteriores fuentes con el objeto de extraer de dicha fusión información de ”mayor calidad”. En el caso en el que las fuentes de datos sean sensores, el marco formal se denomina fusión sensorial (Wald, 1998). Las técnicas y algoritmos más convenientes en cada caso vienen influenciados por: 1. El tipo de aplicación para la que se van a utilizar los resultados del proceso de fusión, que determina: (1) el fenómeno que hay que observar, (2) los sensores utilizados como fuentes de información a priori, y (3) el tipo de razonamiento que se realizará sobre la información recibida por el sistema de fusión. 2. La incertidumbre asociada a la información y la posible discrepancia entre la información proveniente de distintas fuentes. 3. La velocidad de respuesta mı́nima necesaria para el buen funcionamiento del sistema, caracteŕıstica importante en los sistemas de control en tiempo real. Ésta se encuentra directamente relacionada con el tiempo máximo de respuesta de los procesos de fusión y la velocidad de muestreo de las fuentes sensoriales. 4. Las caracteŕısticas propias de la arquitectura de control del sistema y la localización y caracteŕısticas de los sensores. Entre las distintas técnicas presentadas hasta ahora nos interesan para la aplicación que nos ocupa las técnicas dead-reckoning y el sistema DGPS. Aunque la técnica DGPS tiene buena precisión, está expuesta a los problemas de recepción de señales de los satélites por parte del receptor GPS debido a los obstáculos que pueden existir en el entorno natural, y como ya se ha comentado puede haber periodos de tiempo en los que no se puede localizar el robot. Recordemos que el problema de las técnicas dead-reckoning y odometŕıa es el incremento del error con la distancia recorrida. De este modo, ambas técnicas tienen sus limitaciones para utilizarlas en la localización de un robot en exteriores aunque se pueden conseguir buenos resultados al utilizarlas conjuntamente. Con una medida ocasional de la posición absoluta, el error en una estimación de la posición computada utilizando dead-reckoning u odometŕıa no crecerá con la distancia recorrida. Dead-reckoning se utiliza porque se obtienen buenas medidas en los cambios de la posición del robot en cortos periodos de tiempo y también en los casos en los que la posición fija del DGPS no está disponible. Por otro lado el DGPS se utiliza para evitar el 4.4 Fusión Sensorial 81 crecimiento con la distancia del error introducido por las técnicas de localización relativa (dead-reckoning y odometŕıa) (Goel et al., 1999). A partir de estas ideas, aparecen las técnicas de Fusión Sensorial para la estimación de la posición de un robot móvil. Se trata de algoritmos que solucionan el problema de localización combinando información inicial y medidas de la posición relativa y absoluta. Estas técnicas dependen de un método probabiĺıstico, donde los conceptos de incertidumbre y valor probable son comunes. Con estos métodos se consigue mejorar la precisión en el cálculo de la posición del robot. Resultará más adecuado emplear un método u otro dependiendo del tipo de problema y la tarea a desarrollar por el robot. 4.4.1. Filtro de Kalman En 1960, Rudolf Emil Kalman desarrolló un filtro recursivo para solucionar el problema de filtrado linear de datos discretos (Kalman, 1960). Este filtro, conocido como Filtro de Kalman, es un conjunto de ecuaciones matemáticas que proporcionan la estimación del estado de un proceso. En el caso de localización de robots el estado es la pose del robot. También se puede definir como una herramienta matemática para estimar el estado de un sistema dinámico con ruido usando medidas del ruido relacionado con el estado. Se considera que el estado y el ruido se puede representar por una función gaussiana. Una función gaussiana es una función de densidad uni-modal, por lo tanto no se permiten multiples posibilidades sobre la localización. Hay una única estimación de la localización, correspondiente al término medio de la función gaussiana de cada variable de estado, en este caso las coordenadas de la posición y la orientación. El filtro de Kalman procesa todas las medidas disponibles para estimar el estado, tanto las medidas exactas como las inexactas. Además, usa el conocimiento del sistema y de la dinámica sensorial (descripciones probabiĺısticas del ruido del sistema y de las mediciones), y cualquier dato disponible sobre los valores inicales del estado. La idea fundamental del Filtro de Kalman es la actualización. Al ser recursivo, se evita el almacenamiento de todas las observaciones utilizadas en las estimaciones anteriores. Se calculan los errores cuadráticos medios del vector de estado y se actualizan continuamente. El cálculo de la matriz de Kalman minimiza estos errores del vector de estado (Welch y Bishop, 2004). Cuando el sistema no es lineal se puede aplicar el filtro de Kalman sobre una 82 Técnicas de localización en exteriores linealización del sistema. La linealización se puede realizar utilizando el desarrollo de Taylor y, generalmente, basta con tomar hasta los términos de primer orden. Esta variación de la aplicación del Filtro de Kalman se conoce como el Filtro de Kalman Extendido (Roumeliotis y Bekey, 1997). 4.4.2. Método de Montecarlo Existen otros métodos probabiĺısticos de localización que realizan fusión sensorial. Se basan en el cálculo de la función densidad de probabilidad del vector de estado. El más conocido es el método de localización de Monte Carlo (Fox et al., 1999b) que resulta ser una versión de la localización de Markov. La idea principal de la localización de Markov es mantener una densidad de probabilidad, sobre el espacio, de todas las localizaciones del robot en su entorno (Fox et al., 1999a). El método de Monte Carlo es un buen método para localizar eficientemente un robot móvil sin conocer su posición inicial. La función densidad de probabilidad se va actualizando con las medidas obtenidas desde los sensores utilizados. Respecto al Filtro de Kalman, la localización de Monte Carlo presenta la ventaja de que puede localizar un robot globalmente y también puede combinar las ventajas de la localización de Markov basada en redes con la eficiencia y exactitud de las técnicas basada en un filtro de Kalman (Dellaert et al., 1999). 4.4.3. Filtro de part́ıculas Desde la mitad de la década de los noventa, algunos campos de investigación de la robótica ha cambiado hacia técnicas que utilizan modelos imperfectos y que incorporan datos de sensores imprecisos. Se comienza a utilizar el término robótica probabiĺıstica. La robótica probabiĺıstica lleva a cabo esta integración de modelos imperfectos y sensores imperfectos a través de leyes probabilistas tales como reglas Bayessianas. Un filtro de part́ıculas se basa en la robótica probabiĺıstica. Los filtros de part́ıculas comprenden un amplio número de algoritmos de Monte Carlo secuenciales para una conclusión aproximada en cadenas de Markov observables (Doucet et al., 2001). En el área de localización se han conseguidos implantaciones de los filtros de part́ıculas con éxito (Thrun, 2002). Los filtros de part́ıculas son técnicas aproximadas para calcular estados posteriores en cadenas de Markov controlables y observables parcialmente. Normalmente en robótica los filtros de part́ıculas se aplican en espacios de estados continuos. 4.5 Localización cooperativa o multi-robot 83 Las dos caracteŕısticas más importantes que aportan los filtros de part́ıculas son la localización global y la solución del problema de robot raptado. Con el filtro de part́ıculas, mediante las funciones de densidad de probabilidad del vector de estado, se puede calcular la nueva posición del robot. El funcionamiento se basa en ir computando las densidades de probabilidad posibles, y por lo tanto los lugares en los que se puede encontrar el robot a partir de los datos recogidos con los sensores. De esta forma, a medida que se van obteniendo datos de los sensores se va aproximando cada vez mejor la posición del robot al ir anulándose algunas densidades de probabilidad. Se pasa de la incertidumbre global a una buena localización del robot (Thurn, 2002). 4.5. Localización cooperativa o multi-robot Este sistema de estimación de la posición de un robot móvil, que también se ha estado desarrollando en la última década, es una técnica que obtiene muchas ventajas de la utilización de un grupo de robots que cooperan entre śı para completar una tarea. Cada uno de los robots recoge datos de los sensores respecto a su propio movimiento y comparte esta información con el resto del equipo durante los ciclos de actualización. Un estimador simple, puede ser un filtro de Kalman, un filtro de part́ıculas, etc., procesa la información de la posición disponible de todos los miembros del equipo y produce una estimación de la pose para cada uno de ellos. Cada robot lleva a bordo un conjunto de sensores y ejecuta su algoritmo de localización (Madhavan et al., 2001; Howard et al., 2003). También es imprescindible la existencia de comunicación entre todos los miembros del sistema para transferir la información de la posición de cada robot siempre que se produzca el desplazamiento de alguno. Con este método se calcula la posición relativa de cada robot respecto a los demás y se establece un sistema de coordenadas común para el grupo de robots. En este caso aparecen dos problemas de posicionamiento: (a) la estimación de la posición de cada robot y (b) la estimación de la posición relativa de los otros robots del sistema (Moors et al., 2003). Se pueden reducir errores combinando las medidas recogidas por los diferentes robots del sistema. Comprensiblemente, se puede alcanzar una fiabilidad más alta con este sistema que con un solo robot debido a que se tienen más fuentes de información para calcular la posición y orientación de cada uno de los robots. Además, el grupo de robots puede llevar a cabo una tarea incluso si un robot del grupo falla. 84 Técnicas de localización en exteriores Aún aśı, este sistema multi-robot presenta una complejidad elevada comparada con un sistema formado por un simple robot. Existen dos tipos de arquitecturas que se pueden emplear para lograr la cooperación en el grupo de robots: centralizada y descentralizada. En la primera, toda la planificación, control de ejecución y monitorización de las tareas se lleva a cabo por una unidad de control simple. Esta arquitectura conlleva un incremento del coste computacional proporcional al número de robots. La segunda puede resolver los problemas de la primera al descentralizar la arquitectura. En este caso, cada robot acomete sus propias decisiones. Este tipo de arquitectura proporciona robustez y modularidad. Las ventajas que derivan del intercambio de información entre miembros de un grupo son más importantes en el caso de grupos de robots heterogéneos. Cuando un grupo de robots se compone de plataformas diferentes que tienen diferentes tipos de sensores, y por lo tanto diferentes capacidades para localizarse, la calidad de la estimación de la localización variará a través de los miembros individuales significativamente. La comunicación y el flujo de información entre los miembros del grupo constituye una forma de compartir información de los sensores y mejorar la precisión del posicionamiento total. Las limitaciones que presenta esta técnica son: 1. Sólo se permite mover a un robot antes del movimiento de los otros miembros del sistema. 2. Los robots se tienen que mover de tal forma que la comunicación se mantenga constantemente. En el caso de utilizar visión, éstos necesitan “verse” en todo momento. 3. Para llevar a cabo una tarea se exige una gran coordinación en los movimientos de cada robot. Caṕıtulo 5 Fusión sensorial 5.1. Introducción La técnica más utilizada para la localización de robots móviles es la fusión sensorial. A partir de diferentes fuentes de información se integran los datos obtenidos para estimar los parámetros que definen la pose del robot. Los diferentes métodos utilizados para realizar fusión sensorial se basan en técnicas probabiĺısticas (Thrun et al., 2005). En este caṕıtulo se presentan estas técnicas probabiĺısticas y sus diferentes métodos, como los filtros del Kalman y los filtros de part́ıculas. 5.2. Localización Bayesiana La localización bayesiana se basa en técnicas probabiĺısticas para estimar la posición de un robot móvil a partir de las observaciones realizadas por el robot (medidas obtenidas por los sensores a bordo del robot) y las acciones efectuadas para el movimiento del robot. A partir de estas observaciones y acciones se pueden obtener los desplazamientos que tienen lugar. Se realiza una estimación de la función de densidad de la posición del robot xt en el instante t a partir de las observaciones z y acciones realizadas a hasta ese instante, y de la probabilidad a priori. La función de densidad que representa la probabilidad a posteriori después de t instantes de tiempo es p(xt | z1, ..., zt, a1, ..., at−1) (5.1) 86 Fusión sensorial Se consideran las dos siguientes suposiciones para la formulación recursiva de la función de densidad en el instante t. 1. Condición de Markov La condición de Markov establece que el conocimiento de las acciones y posiciones previas no proporciona ninguna información adicional para el conocimiento de la posición y acción inmediatamente previas. Esta condición establece sobre el modelo dinámico que el nuevo estado xt depende únicamente del estado anterior xt−1 y de la acción anterior at−1 p(xt | x1, ..., xt−1, a1, ..., at−1) = p(xt | xt−1, at−1) (5.2) 2. Independencia en las observaciones Las medidas z observadas por el robot son independientes del tiempo y la probabilidad de la observación depende del estado y no de la acción, a, previa p(z1, ..., zt | x1, ..., xt, a1, ..., at) = t∏ i=1 p(zi | xi) (5.3) Por lo tanto consideramos la función de probabilidad conjunta de todas las observaciones como el producto de la función de probabilidad de cada lectura. Se utiliza la regla de Bayes para el cálculo de la probabilidad a posteriori que se puede expresar como la probabilidad de la última medida zt p(zt | xt, z1, ..., zt−1, a1, ..., at−1), ponderada por la función de probabilidad a priori de la posición del robot p(xt | z1, ..., zt−1, a1, ..., at−1). p(xt|z1, ..., zt, a1, ..., at−1) = αp(zt|xt, z1, ..., zt−1, a1, ..., a,t−1 ) p(xt|z1, ..., zt−1, a1, ..., at−1) (5.4) La probabilidad (verosimilitud) de la última observación zt depende únicamente de la posición actual del robot p(zt | xt, z1, ..., zt−1, a1, ..., at−1) = p(zt | xt) (5.5) Este término se denomina modelo de observación. 5.2 Localización Bayesiana 87 La probabilidad a posteriori, p(xt), se puede calcular recursivamente a partir de la probabilidad que representa el modelo de observación, p(zt | xt), de la probabilidad que representa el modelo de movimiento o dinámica del movimiento, p(xt | xt−1, at−1), y la probabilidad a priori, p(xt−1), p(xt) = αp(zt | xt) ∫ xt−1 p(xt | xt−1, at−1)p(xt−1) (5.6) donde α es un factor de normalización tal que ∫ xt p(xt) = 1. Esta expresión genérica se aplica a cualquier implementación. Existen diferentes enfoques de la localización bayesiana que se distinguen según la técnica empleada para estimar computacionalmente la función de densidad anterior. Las técnicas existentes son: 1. Filtros de Kalman: suposición de que p(x) son funciones normales. 2. Filtros de part́ıculas: suposición de que p(x) son funciones multimodales. Las técnicas bayesianas para la localización son potentes en aplicaciones de robots reales. Los principales problemas que se presentan son la complejidad computacional y la necesidad de definir a priori el tamaño de la discretización del espacio de parámetros en part́ıculas. Esta discretización es la base para la aplicación de los filtros de part́ıculas y aśı poder representar los estados continuos del robot discretizando las distribuciones de probabilidad de forma no uniforme. Hay diferentes nombres de métodos basados en este enfoque: Monte Carlo, Condensation e Importance Resampling Filters, aunque en general se utiliza el término filtros de part́ıculas. En el enfoque bayesiano hay que definir un modelo de observación y un modelo de movimiento (Thrun et al., 2005). Por un lado, el modelo de observación permite evaluar la probabilidad de que unas lecturas zt se hayan realizado en una determinada posición xt; y por otro lado, el modelo de movimiento evalúa la probabilidad de que el robot se encuentre en una posición nueva xt a partir de la posición anterior xt−1 y la posición estimada a partir de las acciones realizadas para el movimiento del robot at−1. La robustez y la fiabilidad de la estimación bayesiana dependen de la calidad de estos modelos. 5.2.1. Modelo de observación Un modelo de observación proporciona una estimación de la función de densidad p(z | x) , es decir, la probabilidad de que unas medidas z = (z1, ..., zn) 88 Fusión sensorial en un instante t se adecuen a la configuración x del modelo que se está estimando. Un buen modelo de observación debe describir las probabilidades reales de p(z | x) lo mejor posible. 5.2.2. Modelo de movimiento Los robots móviles pueden conocer la evolución del sistema mediante sensores de posición. El robot va calculando los incrementos de posición medidos desde estos sensores (por ejemplo los encoders ubicados en las articulaciones de las patas), es decir, a partir de las medidas de posición que se obtienen de las acciones realizadas para que se produzca el desplazamiento del robot. Por lo tanto, el modelo de movimiento del robot se puede formular a partir de los datos proporcionados por estos sensores de posición. Este modelo define la probabilidad de localización del robot desde una localización anterior y los datos de los sensores de posición. 5.3. Filtro de Kalman En 1960, R.E. Kalman publicó su famoso trabajo describiendo una solución recursiva al problema de filtrado lineal de datos discretos (Kalman, 1960). Desde este instante, debido en gran parte a los avances de la computación digital, el filtro de Kalman (KF)1 ha sido objeto de una extensa investigación y aplicación, particularmente en el área de robots autónomos o navegación asistida (Longo et al., 2002; Catalbiano et al., 2004). El KF es un conjunto de ecuaciones matemáticas que proporcionan términos medios computacionales (recursivos) eficientes para estimar el estado de un proceso, de forma que minimiza el término medio del error cuadrático (Maybeck, 1979; Gelb et al., 1986; Grewal y Andrews, 1993; Welch y Bishop, 2004; Thrun et al., 2005). Este filtro es muy potente en varios aspectos: soporta estimaciones del pasado, presente e incluso estados del futuro, y también puede hacerlo cuando la precisión del sistema modelado es desconocida. El KF es un medio para calcular la probabilidad que se representa mediante una distribución gaussiana. Formalmente, el KF estima la probabilidad condicional de que el sistema esté en el estado xt a partir de las acciones a1, ..., at y las medidas z1, ..., zt, es decir, estimar la ecuación 5.1. En la expresión 5.6 podemos ver los términos que hay que estimar. Por lo tanto se necesita encontrar 1Denominamos al filtro de Kalman como KF debido al término inglés Kalman filter. 5.3 Filtro de Kalman 89 expresiones para estimar el modelo del sistema p(xt | xt−1, at−1) y el modelo de medición p(zt | xt). 5.3.1. Sistema dinámico lineal El KF asume que el estado y las medidas del sistema pueden ser descritas por un sistema dinámico lineal (Maybeck, 1979; Grewal y Andrews, 1993). Por lo tanto, el sistema toma un modelo lineal del sistema y de medición. Modelo del sistema. El KF asume que el estado del sistema evoluciona según la ecuación lineal: xt = Axt−1 + wk−1 (5.7) El estado en el instante t depende del estado del sistema en el instante t− 1 y del ruido del sistema wt−1. La matriz A es una matriz n × n que relaciona el estado del sistema en el instante t− 1 con el estado en el instante t. Modelo de medición. Este modelo describe cómo se relacionan las mediciones con los estados. Se trata de estimar la medición a partir del estado del sistema y aśı compararla con la medición real. El KF asume que las medidas se pueden modelar por una ecuación que relaciona linealmente el estado del sistema xt y una medida zt. Es decir, zt = Hxt + vt (5.8) La matriz H es una matriz n×m que modela lo que debeŕıa de ser la medida real sin ruido. vt representa el ruido de medición. Al considerar distribuciones gaussianas, se considera que tanto wt como vt son variables aleatorias independientes y con término medio cero, por lo tanto: wt ∼ N(0, Qt) y vt ∼ N(0, Rt)2. También se considera que el estado inicial x0 es independiente y descrito por una distribución gaussiana. A partir de estás consideraciones se pueden derivar las ecuaciones del algoritmo de un filtro de Kalman lineal (Negenborn, 2003). Este filtro se compone de una fase de estimación y una fase de actualización. Fase de estimación En esta fase se estima en cada instante el estado del sistema a partir de las ecuaciones de predicción. Estas ecuaciones son: 2N(µ, Σ) representa la función gaussiana con término medio µ y covarianza Σ. 90 Fusión sensorial x̂−k = Ax̂+ k−1 (5.9) P− k = AP+ k−1A T + Qk−1 (5.10) El KF calcula la estimación el estado a priori x̂−k a partir del último estado a posteriori x̂+ k−1 y el modelo del sistema. La evolución del sistema también está sujeta a ruido y éste puede aumentar el error en la estimación del estado a priori P− k . El primer término propaga el error de la estimación del último estado a posteriori al error de la estimación del estado actual a priori. El segundo término es el ruido del sistema que se introduce en el estado del sistema en cada instante. Fase de actualización Esta fase se utiliza cuando hay una medición. Las ecuaciones son las siguientes: x̂+ k = x̂−k + Kk(zk −Hx̂−k ) (5.11) P+ k = (I −KkH)P− k (5.12) El KF calcula la estimación del estado a posteriori x+ k teniendo en cuenta la estimación del estado a priori x−k y combinando esto con la matriz de Kalman (Kk) y la diferencia entre la medición zk y una predicción de la medición Hx̂−k . A la matriz Kk se le denomina matriz de ganancia de Kalman (o matriz de Kalman). Esta matriz determina cuánto debeŕıa tenerse en cuenta el término zk − Hx̂−k en la estimación del estado a posteriori. Se calcula a partir del error relativo entre el error de la estimación del estado a priori P− k y el ruido del sistema de medición Rk. Para comparar ambos errores se utiliza la matriz HT . La matriz Kk viene dada por: Kk = P− k HT (HP− k HT + Rk)−1 (5.13) Cuando el ruido del sistema de medición tiende a cero, es decir, ĺım Rk→0 Kk = H−1, (5.14) el KF conf́ıa menos en el modelo del sistema y más en el modelo de medición. En cambio, cuando el error a priori P− k tiende a cero, es decir, 5.3 Filtro de Kalman 91 ĺım P− k →0 Kk = 0, (5.15) el KF da mayor peso al modelo del sistema que al modelo de medición. El KF también usa la matriz de Kalman para actualizar el error de la estimación del estado a posteriori P+ k , como se puede observar en la ecuación 5.12. En definitiva, la matriz de Kalman juega un papel muy importante en el KF porque minimiza el error en las estimaciones de los estados. 5.3.2. Sistema dinámico no lineal Muchos sistemas dinámicos no son completamente lineales. Esto significa que las funciones que describen el estado del sistema son no lineales. Cuando ocurre esto no se puede aplicar el KF lineal, por lo tanto se utiliza un filtro de Kalman extendido (EKF, Extended Kalman Filter). Este tipo de filtrado consiste en linealizar previamente las ecuaciones del filtro de Kalman. Este proceso se lleva a cabo a partir de la matriz de transición de estados. Esta matriz representa un modelo cinemático del sistema, y describe la transición desde el estado k − 1 al estado k. El sistema se linealiza tomando el jacobiano de la matriz de transición de estados, es decir, su derivada primera. Sin embargo, si las no linealidades son suficientemente importantes, el error de estimación se puede reducir significativamente a través del uso de un orden más alto del desarrollo de Taylor para linealizar el problema (Roumeliotis y Bekey, 1997). Las ecuaciones que describen el comportamiento del sistema y de la medición en el caso caso no lineal son las siguientes. Modelo del sistema no-lineal. La ecuación que describe el sistema es: xk = f(xk−1) + wk−1 (5.16) donde f es una función del sistema no lineal que relaciona el estado anterior con el estado actual, y donde wk−1 representa el ruido del sistema. Este ruido se considera descrito con una distribución gaussiana, independiente y con término medio cero. Modelo de medición no lineal. Cuando el sistema de medición no es lineal se describe por la siguiente ecuación : zk = h(xk) + vk (5.17) 92 Fusión sensorial donde h es una función de medición no lineal que relaciona el estado del sistema con una medición, y donde vk es el ruido de la medición. Al igual que en el caso anterior, este ruido tiene una distribución gaussiana, es independiente y con término medio cero. Estos cambios en los modelos del sistema y observación provocan también cambios en las ecuaciones que representan el EKF respecto a las que representan el KF. Fase de estimación En esta fase el EKF propaga el estado y el error del sistema del instante anterior al instante actual usando las ecuaciones de predicción: x̂−k = f(x̂+ k−1) (5.18) P− k = AkPk−1A T k + Qk−1 (5.19) donde la matriz jacobiano Ak contiene las derivadas parciales de la función del sistema f con respecto a la estimación del estado a posteriori del instante anterior x+ k−1, Ak = ∂f(x) ∂x ∣∣∣∣ x=x̂+ k−1 (5.20) Fase de actualización El EKF actualiza la estimación del estado a priori con una medición zk mediante las ecuaciones de actualización: Kk = P− k HT k (HkP − k HT k + Rk)−1 (5.21) x̂+ k = x̂−k + Kk(zk − h(x̂−k )) (5.22) P+ k = (I −KkHk)P− k (5.23) donde la matriz jacobiano Hk contiene las derivadas parciales de la función medición h respecto a la estimación del estado a priori x−k , Hk = ∂h(x) ∂x ∣∣∣∣ x=x̂− k (5.24) 5.4 Filtro de part́ıculas 93 La aplicación del KF presenta diferentes problemas debido a que en robótica móvil los supuestos de optimización del KF no se cumplen. El modelo de movimiento y el modelo observacional son normalmente no lineales, por lo tanto la linealidad que se aplica en el KF no se cumple. Otro problema que aparece en el KF está relacionado con la densidad de probabilidad, éste considera que las densidades de probabilidad correspondientes a los errores de ambos modelos son Gausianas, y esto no se cumple normalmente. Al aplicar el EKF el problema de la linealidad se resuelve ya que se pueden tomar en cuenta las no-linealidades de los procesos al linealizarlos en torno a su punto de operación en cada instante. Aún aśı, no se pueden resolver los problemas de la hipótesis de gaussianidad y además su cálculo es computacionalmente costoso. Los filtros de part́ıculas permiten resolver los problemas asociados a los filtros de Kalman. Éstos no asumen la linealidad y gaussianidad en los procesos y su cálculo es computacionalmente eficiente. 5.4. Filtro de part́ıculas El concepto de los filtros de part́ıculas aparece para solucionar un problema al que se enfrenta la localización bayesiana: ¿cómo representar la función de probabilidad a posteriori? Esta es la función que estima las posiciones más probables del robot y además normalmente será multimodal, es decir, existirán diferentes posiciones con una probabilidad alta de que el robot se encuentre simultáneamente en ellas (Thrun et al., 2005). Los filtros de part́ıculas solucionan este problema discretizando el espacio de estados (todas las posibles posiciones del robot en el espacio). Esta discretización se lleva a cabo tomando celdillas en todo el espacio donde es posible que se encuentre el robot y la posición del robot se representa como una part́ıcula en cada celdilla. Estos algoritmos de localización calculan la probabilidad de que el robot esté situado en cada una de las celdillas utilizando el modelo bayesiano, por lo tanto se calcula la probabilidad de cada part́ıcula. El nombre de filtro de part́ıculas se refiere al filtro que se realiza para despreciar las part́ıculas cuya probabilidad es despreciable a la hora de describir la posición del robot. Esta sección presenta a modo de ejemplo un algoritmo de estimación basado en un filtro de part́ıculas para calcular la densidad de probabilidad a posteriori p(xt | Zt, At−1) que define el estado del robot (posición y orientación en este caso). Esta densidad de probabilidad representa el estado del robot en el instante actual, xt, dadas unas observaciones realizadas por sus sensores, Zt = {z1, ..., zt}, 94 Fusión sensorial y una secuencia de acciones realizadas, At−1 = {a1, ..., at−1}, que representan las lecturas de los sensores de posición al producirse la acción de los motores. El cálculo de p(xt | Zt, At−1) se puede descomponer en dos pasos: predicción e integración de las observaciones. 1. Predicción Se utiliza el modelo de movimiento p(xt | xt−1, at−1) para predecir la posición del robot en el instante actual con los datos at−1, a partir de la densidad de probabilidad del instante anterior p(xt−1 | Zt−1, At−1). p(xt | Zt−1, At−1) = ∫ t−1 p(xt | xt−1, at−1)p(xt−1 | Zt−1, At−1) (5.25) 2. Integración de las observaciones Se integran las observaciones realizadas en el instante actual, Zt. Se utiliza el modelo de observación p(zt | xt) y se ponderan las probabilidades obtenidas por la fase anterior p(xt | Zt−1, At−1) en función de la probabilidad (verosimilitud) de las medidas Zt, para obtener la densidad a posteriori p(xt | Zt, At−1) = αtp(zt | xt)p(xt | Zt−1, At−1) (5.26) La constante αt es un factor tal que ∫ xt p(xt) = 1 . Existen diferentes métodos para representar esta densidad de probabilidad (filtros de Kalman, filtros de part́ıculas,. . . ). La solución que se plantea es la representación de la densidad mediante un conjunto de muestras o part́ıculas extráıdas de dicha distribución. Se trata de un filtro en el que la densidad a posteriori se presenta mediante una distribución de part́ıculas en el espacio de estados. Estas propuestas propagan las part́ıculas (muestras de la función de densidad a posteriori) utilizando el modelo de movimiento p(xt | xt−1, at) y el modelo de observación p(zt | xt), de forma que el peso combinado de las part́ıculas de una región aproxima la integral de la función de densidad a posteriori en esa región. Este filtro representa la densidad a posteriori mediante un conjunto de N muestras o part́ıculas (m1, ..., mN ) y sus probabilidades asociadas (π1, ..., πN ). Inicialmente, el conjunto de muestras se escoge a partir de la distribución a priori p(x0). Si no existe información a priori, entonces las muestras se 5.4 Filtro de part́ıculas 95 Figura 5.1: Funcionamiento del algoritmo de localización del filtro de part́ıculas. La figura supone que las muestras están estimando un único parámetro, distribuido en el eje horizontal. Las muestras se representan por ćırculos centrados en el valor del parámetro que definen. El área de los ćırculos representa el peso de cada muestra. distribuyen uniformemente por el espacio de estados. Posteriormente, en cada instante de tiempo t, se actualizan las N muestras en función de la acción anterior at−1 y la observación actual zt. Para ello, (fase 1 del algoritmo) se aplica el modelo de movimiento p(xt | xt−1, at−1) a cada una de las N muestras, generando un nuevo conjunto de muestras. Estas muestras representan la predicción de la variable de estado, sin considerar la observación. Para considerar la observación (fase 2 del algoritmo), se obtiene el peso πi asociado a cada muestra según la verosimilitud de que la observación haya sido realizada en un estado del sistema definido por la muestra, p(zt | xt). En un último paso (fase 3 del algoritmo), se remuestrea el conjunto de muestras, extrayendo (con reemplazo) N muestras del conjunto actual, con probabilidad proporcional al peso de cada una. En este nuevo conjunto, por ejemplo, desaparecerán las muestras para las que no hay evidencia de verosimilitud. Una vez construido el nuevo conjunto de muestras, se escalan los pesos asociados a cada una para que representen la probabilidad asociada a cada muestra. Este nuevo conjunto de muestras constituye una representación muestral de la probabilidad a posteriori. En la Figura 5.1 se explica gráficamente la evolución de las muestras en cada fase. 96 Fusión sensorial Caṕıtulo 6 Algoritmo de localización para el Robot SILO6 6.1. Introducción Este caṕıtulo presenta el algoritmo implementado en el robot SILO6 para llevar a cabo la tarea de localización y está particularizado, por tanto, para el sistema sensorial empleado que consta de: encoders, brújula electromagnética, inclinómetro y DGPS. El algoritmo de localización utilizado es un EKF clásico que se compone de una fase de estimación (aplicando técnicas dead-recknoning) y una fase de actualización (utilizando DGPS). El error que se produce en las medidas de la posición se calcula en ambas fases, pero durante la fase de actualización los errores obtenidos influyen en el cálculo y minimización del error del EKF. Por esta razón, la fase de actualización es más importante para computar la posición del robot que la fase de estimación, y por lo tanto requiere un sensor más preciso. El método que introduce mayores errores (técnicas dead-reckoning) se usa durante la fase de estimación mientras que el método con errores menores, DGPS, se aplica en la fase de actualización. Es importante resaltar que la posición del robot se computa en ambas fases, mientras que la orientación del robot se obtiene a partir de los datos de la brújula electromagnética sólo durante la fase de estimación (Roumeliotis y Bekey, 1997). En este caṕıtulo se explican las dos formas diferentes en las que se han utilizado técnicas dead-reckoning en la fase de estimación del EKF implementado. Por un lado, se aplican estas técnicas con la información proporcionada por los 98 Algoritmo de localización para el Robot SILO6 encoders y la brújula electromagnética para estimar la posición y orientación del robot. Por otro lado, se aplica el mismo método añadiendo un tercer sensor que consiste en una pareja de inclinómetros. Téngase en cuenta que con el DGPS también se dispone de otra fuente de información para estimar la posición del robot. 6.2. Fase de estimación El estado del robot se define por el vector [X1(k), X2(k), X3(k)]T (véase la Figura 6.1), donde X1(k) y X2(k) son las componentes horizontales de posición del robot en el sistema de referencia fijo en el instante k y se estiman usando técnicas dead-reckoning. X3(k) es la orientación del robot, θ, obtenida filtrando las lecturas de la brújula electromagnética, α, debido a que se han apreciado grandes fluctuaciones en sus datos. La posición y orientación estimadas en el instante k con la información obtenida en el instante k-1, X(k, k−1), se expresan como: X(k, k − 1) =   X1(k, k − 1) X2(k, k − 1) X3(k, k − 1)   =   X1(k) + v(k)T cos(X3(k, k − 1)) X2(k) + v(k)T sin(X3(k, k − 1)) θ(k, k − 1, k − 2)   (6.1) donde v(k) es la velocidad del robot y T es el instante de tiempo. En nuestro sistema la velocidad del robot, v, y la orientación del robot, θ, son inexactas; por lo tanto, la medida de X(k, k − 1) obtenida con (6.1) es muy imprecisa. La velocidad v es aquella a la que debe desplazarse el robot, pero esta velocidad no se corresponde exactamente con la que se obtiene en cada instante como se puede apreciar en la Figura 6.2, que representa la velocidad deseada y la velocidad estimada en función del tiempo. Generalmente los datos se toman cada segundo, pero existen situaciones en las que esto no ocurre y la medida se toma con dos segundos de diferencia con la anterior. Esto provoca más inexactitud en la estimación de la velocidad. Sin embargo, cuando se obtienen los incrementos del vector de estado en el sistema de referencia absoluto, ∆X1(k) e ∆X2(k), usando técnicas dead-reckoning se consiguen mejores resultados ya que la estimación de estos incrementos es mejor que la estimación de v. De esta forma, se puede calcular el vector de estado como: 6.2 Fase de estimación 99 Y X X (k-1)2X (k)2 X (k-1)1 X (k)1 X (k)3 X (k-1)3 Figura 6.1: Vector de estado en el sistema de referencia fijo. X(k, k − 1) =   X1(k, k − 1) X2(k, k − 1) X3(k, k − 1)   =   X1(k − 1) + ∆X1(k) X2(k − 1) + ∆X2(k) θ(k, k − 1, k − 2)   (6.2) donde ∆X1 y ∆X2 son los incrementos de la posición en el sistema de referencia fijo (Gelb et al., 1986). Las ecuaciones (6.1) y (6.2) son equivalentes; por razones de la exactitud usaremos (6.2) para obtener el vector de estado, y por razones de simplicidad usaremos (6.1) para obtener la covarianza del vector de estado. Para calcular los incrementos de posición a partir de técnicas dead-reckoning se utilizan diferentes grupos de sensores. Por un lado, se consideran los datos obtenidos de los sensores de posición (encoders) y la brújula electromagnética. Por otro lado, se utilizan encoders, la brújula electromagnética y un inclinómetro. En la sección 6.2.2 se explica el porqué del uso de este último sensor. 100 Algoritmo de localización para el Robot SILO6 0 50 100 150 200 250 300 350 400 450 0 0.005 0.01 0.015 0.02 0.025 0.03 Medidas V el oc id ad ( m /s ) Figura 6.2: Velocidad del robot SILO6: velocidad teórica (ĺınea roja) y velocidad estimada (ĺınea azul). 6.2.1. Dead-reckoning con encoders y brújula electromagnética. En esta sección se explica cómo se calculan los incrementos de posición aplicando técnicas dead-reckoning con la información obtenida de los encoders y la brújula electromagnética. Los encoders son sensores de posición situados en las articulaciones de las patas el robot que proporcionan información incremental (véase la sección 3.2.1.4). A partir de éstos se puede calcular el desplazamiento que ha tenido lugar respecto a la posición anterior. El cálculo de la posición del robot a partir de esta información incremental se mejora conociendo la orientación del robot. De ah́ı el uso de la brújula electromagnética. Se han observado grandes variaciones en la brújula electromagnética durante la realización de los experimentos, lo que supone un problema importante para estimar la orientación del robot. Esta orientación se utiliza para calcular los incrementos de posición que realiza el robot respecto al sistema de referencia fijo. Las variaciones en los datos se deben al ruido eléctrico generado en la brújula electromagnética y las fluctuaciones son causadas por el movimiento y las vibraciones del robot. Este problema se soluciona usando un filtro de segundo 6.2 Fase de estimación 101 0 500 1000 1500 2000 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Medidas O rie nt ac ió n (r ad ) Figura 6.3: Señal de la orientación después de aplicar un filtro de 2o orden: la señal de entrada al filtro (brújula electromagnética) se representa en color azul, la salida para los parámetros del filtro utilizado, γ = 0.6, β = 0.35 y λ = 0.05, se representa en rojo. orden, θ(k, k− 1, k− 2), definido en 6.3. La aplicación del filtro proporciona una variación suave de los datos de la brújula electromagnética mientras el robot se desplaza (véase la Figura 6.3). Esta variación suave es importante porque la velocidad a la que se desplaza el robot es pequeña y las variaciones de la orientación no son bruscas, por lo tanto este filtro describe bien la evolución de la orientación a lo largo del desplazamiento del robot. El filtro de segundo orden utilizado es: θ(k, k − 1, k − 2) = γX3(k − 2) + βX3(k − 1) + λα(k) (6.3) donde γ = 0.60, β = 0.35 y λ = 0.05 se han determinado experimentalmente. Obsérvese que el filtro, obtenido emṕıricamente, tiene en cuenta los datos X3(k− 2), X3(k− 1) y α(k) para calcular la orientación en el instante k y que se da más peso a los datos previos de la orientación para evitar variaciones grandes en los datos de la brújula electromagnética. En la Figura 6.4 podemos ver el resultado obtenido para otro tipo de filtros (de primer y segundo orden) y se puede comprobar el mejor ajuste de los parámetros γ, β y λ seleccionados (indicado en 102 Algoritmo de localización para el Robot SILO6 0 200 400 600 800 1000 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Medidas O rie nt ac io n (r ad ) Figura 6.4: Señal de la orientación después de aplicar un filtro de 2o orden: la señal de entrada al filtro (brújula electromagnética) se representa en azul, la salida para los parámetros del filtro γ = 0, β = 0.9 y λ = 0.1 se representa en amarillo, para los parámetros del filtro γ = 0.5, β = 0.35 y λ = 0.15 se representa en verde, para los parámetros del filtro γ = 0.5, β = 0.4 y λ = 0.1 se representa en negro, para los parámetros del filtro γ = 0.6, β = 0.35 y λ = 0.05 se representa en rojo. rojo). Con objeto de evitar grandes fluctuaciones en los valores filtrados se aplica otra condición. Se trata de no tener en cuenta el valor filtrado obtenido si la diferencia entre el actual, X3(k), y el anterior, X3(k − 1), supera un ĺımite. Este ĺımite se fija experimentalmente en 5o, es decir: X3(k) = { X3(k) si |X3(k)−X3(k − 1)| ≤ 5 X3(k) si |X3(k)−X3(k − 1)| > 5 (6.4) De esta forma se evita tener en cuenta cambios bruscos de la orientación ya que a la velocidad que se desplaza el robot no es probable que esto tenga lugar. La matriz covariante del error de la posición y orientación, P , se obtiene a partir del error introducido por la técnica dead-reckoning, Co, y la variación de la transición de un estado a otro. Esta variación se representa por el jacobiano, que se usa para linealizar el sistema: 6.2 Fase de estimación 103 P (k, k − 1) = J(k, k − 1)P (k − 1)JT (k, k − 1) + Co(k) (6.5) donde J representa el Jacobiano del sistema (6.1). La matriz del Jacobiano se calcula como: J =   ∂X1(k,k−1) ∂X1 ∂X1(k,k−1) ∂X2 ∂X1(k,k−1) ∂X3 ∂X2(k,k−1) ∂X1 ∂X2(k,k−1) ∂X2 ∂X2(k,k−1) ∂X3 ∂X3(k,k−1) ∂X1 ∂X3(k,k−1) ∂X2 ∂X3(k,k−1) ∂X3   =   1 0 −Tv(k) sin X3(k, k − 1) 0 1 Tv(k) cosX3(k, k − 1) 0 0 λ   (6.6) En (6.5), Co representa el error, en cada medida, introducido por los sensores usados en la fase de estimación (encoders y brújula electromagnética). Este error introducido en cada medida es importante debido a la influencia que tiene sobre la matriz P . La matriz Co se obtiene emṕıricamente. No se considera el error de correlación entre las diferentes componentes porque es despreciable respecto a cada componente. El cálculo del error de las componentes X1 y X2 se basa en el error introducido en la posición a partir de técnicas dead-reckoning, y el error de la componente X3 se calcula a partir de los datos de la brújula electromagnética. La matriz Co que se obtiene es la siguiente: Co =   0.0047 · cos(θ(k)) 0 0 0 0.0047 · sin(θ(k)) 0 0 0 0.0349   (6.7) Esta matriz está referida al sistema de referencia fijo. El error de cada componente dependerá de la dirección del desplazamiento del robot, por esta razón, el error en las componentes X1 y X2 dependen de la orientación θ. También hay que aclarar que los errores calculados emṕıricamente dependen de elementos tales como la posición de las holguras de las articulaciones, las flexiones en las articulaciones y eslabones, el tipo de terreno y los deslizamientos en los pies. 6.2.2. Dead-reckoning con encoders, brújula electromagnética e inclinómetro. En esta sección se presenta el cálculo de los incrementos de posición aplicando técnicas dead-reckoning con la información obtenida de los encoders, la brújula 104 Algoritmo de localización para el Robot SILO6 Figura 6.5: Inclinómetro: AccuStar II Dual Axis Clinometer. electromagnética y el inclinómetro. Se ha utilizado un inclinómetro denominado AccuStar II Dual Axis Clinometer (véase la Figura 6.5). Las caracteŕısticas de este inclinómetro son: Alimentación: 9 V CC. Corriente: 0.0035 Ap máximo. El sensor opera en posición horizontal y está diseñado para medir el ángulo de cabeceo y de balanceo. Los ĺımites de la temperatura de operación son de -40o a 85o C. Resolución: 0.01o. Rango: ±20o. Un problema fundamental en la tarea de localización en robots caminantes es la presencia de holguras en las articulaciones. Éstas causan desplazamientos transversales no deseados del robot durante su movimiento (véase la Figura 6.6); por lo tanto, se introducen errores en la estimación de la posición del robot. Este 6.2 Fase de estimación 105 Balanceo Desplazamiento Holgura Figura 6.6: Desplazamiento transversal debido a holguras en las articulaciones de las patas del robot. Pie en apoyo Pie en transferencia Apoyo Trípode 1 Trípode 2 Figura 6.7: Estados del robot. efecto es más importante cuando tiene lugar la transferencia de un tŕıpode1 y se observa, sobre todo, en las patas centrales, patas 3 y 4 en el robot SILO6 (véase la Figura 6.7), porque son las que ejercen las fuerzas más elevadas durante la fase de apoyo. Para corregir este efecto hay que estimar los desplazamientos no deseados que se producen en las transferencias de tŕıpodes. Los encoders y la brújula electromagnética no detectan este desplazamiento transversal, por lo tanto la estimación de la posición calculada con las técnicas dead-reckoning presentadas en la sección 6.2.1 no detecta este fenómeno. Sin embargo, el DGPS describe bien este desplazamiento transversal del cuerpo el robot. Por lo tanto la estimación de estos desplazamientos se puede mejorar en la fase de estimación del EKF y, de esta forma, se mejorará también la estimación de la posición obtenida. La solución propuesta en esta tesis doctoral para estimar el desplazamiento transversal del cuerpo del robot se basa en la necesidad de información adicional 1El modo de caminar utilizado se conoce como modo de tŕıpodes alternados (véase la sección 1.2.2). 106 Algoritmo de localización para el Robot SILO6 0 1000 2000 3000 4000 5000 6000 0.02 0.025 0.03 0.035 0.04 0.045 0.05 0.055 0.06 0.065 0.07 Medidas B al an ce o (r ad ) Figura 6.8: Evolución del balanceo del cuerpo del robot: “apoyo” (puntos negros), “tŕıpode 1” (puntos rojos) y “tŕıpode 2” (puntos azules). desde otro tipo de sensor tal como un inclinómetro (Cobano et al., 2006). Un inclinómetro de dos ejes nos proporciona información sobre el ángulo de cabeceo (pitch) y el ángulo de balanceo (roll) del cuerpo del robot. El cabeceo es el ángulo de giro alrededor del eje y del sistema de referencia del cuerpo del robot mientras que el balanceo es el ángulo de giro alrededor del eje x de dicho sistema de referencia del cuerpo del robot (véase la Figura 4.1). Se ha comprobado experimentalmente que la variación del balanceo cuando se producen los cambios de estados en el modo de caminar del robot (véase la Figura 6.8) es una fuente de información para describir los desplazamientos transversales del cuerpo del robot debidos a las holguras. En estos casos, esta variación da más información que la variación del cabeceo. Por lo tanto, solo se tienen en cuenta las lecturas del balanceo para estimar el desplazamiento transversal que sufre el cuerpo del robot debido a las holguras. Los estados del robot son: “apoyo” (todas las patas en apoyo sobre el suelo), “tŕıpode 1” (las patas 1, 4 y 5 en transferencia) y “tŕıpode 2” (las patas 2, 3 y 6 en transferencia). Estos estados se pueden observar en la Figura 6.7. Al igual que ocurre con la brújula electromagnética, las lecturas del inclinómetro tienen grandes fluctuaciones y es preciso filtrarlas para obtener los 6.2 Fase de estimación 107 0 200 400 600 800 1000 0.02 0.03 0.04 0.05 0.06 Medidas B al an ce o (r ad ) 0 200 400 600 800 1000 −0.03 −0.02 −0.01 0 0.01 0.02 Medidas C ab ec eo ( ra d) a) b) Figura 6.9: Señal del inclinómetro después de aplicar un filtro de 2o orden: la señal de entrada al filtro (inclinómetro) se representa en azul, la salida para los parámetros del filtro γ = 0, β = 0.9 y λ = 0.1 se representa en amarillo, para los parámetros del filtro γ = 0.5, β = 0.4 y λ = 0.1 se representa en verde, para los parámetros del filtro γ = 0.6, β = 0.35 y λ = 0.05 se representa en rojo. a) Cabeceo, b) Balanceo. valores del cabeceo y balanceo. La Figura 6.9 muestra la señal obtenida en el inclinómetro (en azul) y las salidas después de aplicar un filtro de 2o orden con diferentes parámetros. Los parámetros del filtro son los mismos que los utilizados en el filtro aplicado a las lecturas de la brújula electromagnética definido en (6.3)(véase la Figura 6.10), además, se aplica la misma condición para evitar las grandes fluctuaciones. Cuando la diferencia entre el ángulo filtrado en el instante k−1 y el ángulo actual filtrado en el instante k es mayor que 5o, el ángulo actual se iguala al ángulo anterior. Este valor también se fija experimentalmente. La estimación de este desplazamiento transversal consta de tres pasos que se detallan a continuación. 108 Algoritmo de localización para el Robot SILO6 0 500 1000 1500 2000 −0.04 −0.02 0 0.02 Medidas C ab ec eo ( ra d) 0 500 1000 1500 2000 0.02 0.04 0.06 0.08 Medidas B al an ce o (r ad ) a) b) Figura 6.10: Señal del inclinómetro después de aplicar un filtro de 2o orden: la señal de entrada al filtro (inclinómetro) se representa en color azul, la salida para los parámetros del filtro utilizado, γ = 0.6, β = 0.35 y λ = 0.05, se representa en rojo. a) Cabeceo, b) Balanceo. Paso 1. Se observa el estado2 del modo de caminar del robot: “apoyo”, “tŕıpode 1” y “tŕıpode 2” (véase la Figura 6.7). Paso 2. Se comprueba si ha tenido lugar algún cambio de estado. Este cambio puede ser: de “apoyo” a “tŕıpode 1”, de “apoyo” a “tŕıpode 2”, de “tŕıpode 1” a “apoyo” o de “tŕıpode 2” a “apoyo” (véase la Figura 6.11). Cuando ocurre un cambio de estado, se computa la diferencia entre el último valor del balanceo en el estado anterior y el último valor del balanceo en el estado actual. 2Hay que diferenciar entre el vector de estado que define la posición y orientación del robot en cada instante y el estado del modo de caminar del robot. 6.2 Fase de estimación 109 Trípode 1 Apoyo Trípode 2 Figura 6.11: Cambios de estado posibles en el modo de caminar del robot SILO6. Hay que indicar que no se tiene en cuenta la evolución del balanceo en un estado, sino solo la diferencia del balanceo entre diferentes estados. Paso 3. Se calcula el desplazamiento transversal, d, que sufre el robot a partir de la variación del balanceo, θbalanceo, calculada en el paso 2 como: d = Kd(θbalanceo − θumbral) (6.8) donde Kd = 1, 37 y se ha computado emṕıricamente. El desplazamiento se producirá hacia un lado u otro del cuerpo del robot dependiendo del signo de la diferencia entre estados del balanceo. Este desplazamiento se computa cuando la diferencia del balanceo es mayor que un determinado umbral, θumbral, que es diferente para cada estado, y se calcula experimentalmente cuando el cuerpo del robot se mantiene apoyado y nivelado sobre un soporte (véase la Figura 6.23) mientras se produce el movimiento de las patas sin apoyar en el suelo. Estos umbrales se utilizan para que las diferencias producidas por el ruido del inclinómetro no se tengan en cuenta al calcular el desplazamiento transversal producido. En la Figura 6.12 se observan las diferencias obtenidas del balanceo para calcular el umbral en cada uno de los tres estados. Nótese que el número de estados “apoyo” es el doble que el número de estados “tŕıpode”. A partir de estas diferencias se computa cada umbral tomando la media de las diferentes medidas obtenidas en cada estado. Una vez que se han calculado los incrementos de posición en el sistema de referencia fijo, ∆X1(k) y ∆X2(k), y la orientación, X3(k), se puede llevar a cabo 110 Algoritmo de localización para el Robot SILO6 0 20 40 60 80 100 120 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 x 10 −3 Cambios de estado V ar ia ci ón d el B al an ce o (r ad ) Figura 6.12: Variación del balanceo para el cálculo del umbral de cada estado: “apoyo” (ĺınea negra), “tŕıpode 1” (ĺınea roja) y “tŕıpode 2” (ĺınea azul). la fase de estimación del EKF a partir de (6.1)-(6.6) como se estableció en la sección 6.2.1. La ecuación (6.7) es diferente en este caso debido a que el error introducido en la estimación de la posición del robot tiene en cuenta el ángulo de balanceo proporcionado por el inclinómetro. En este caso la matriz Co que se obtiene es: Co =   0.0040 · cos(θ(k)) 0 0 0 0.0040 · sin(θ(k)) 0 0 0 0.0349   (6.9) Esta matriz Co tiene las mismas caracteŕısticas que la ecuación (6.7). Por lo tanto, esta matriz Co también dependerá del estado del robot 6.3 Fase de actualización 111 6.3. Fase de actualización La fase de actualización consiste en medir y actualizar la posición del robot usando los datos del DGPS. En esta fase se obtiene el vector de medición Z(k) = (Z1(k), Z2(k))T a partir del DGPS, que nos da una estimación de la posición como X(k) = (X1(k), X2(k))T . De esta forma se pueden comparar las posiciones estimadas X1 y X2 con las posiciones Z1 y Z2. Para actualizar la posición y orientación del robot, X, se debe estimar el vector de medición. El cálculo de este vector se basa en la matriz de medición, H, y la posición calculada durante la fase de estimación, X(k, k − 1). En este caso, las componentes del vector de medición estimado Z(k, k−1) se inician a las componentes X1 y X2, debido a que el vector de medición no se puede calcular usando otro método. Por lo tanto, Z(k, k − 1) = HX(k, k − 1) (6.10) donde H viene dada por: H = ( 1 0 0 1 0 0 ) (6.11) La matriz de Kalman se calcula a partir de la expresión (Gelb et al., 1986): K(k) = P (k, k − 1)HT [ HP (k, k − 1)HT + Cg(k) ]−1 (6.12) En este expresión Cg es la matriz que representa el error introducido por la medida del DGPS, Cg = ( σ2 1 0 0 σ2 2 ) (6.13) donde σ1 y σ2 son la desviación estándar de cada componente dependiendo del modo en el cuál se recogen los datos de la posición del DGPS (indicador de calidad de la medida). El indicador de calidad o modo, ρ, proporciona información relacionada con la recepción de las correcciones diferenciales, concretamente, la forma en la que son recibidas dichas correcciones. Dependiendo de este indicador, la medidas realizadas con el DGPS tendrán una desviación estándar u otra, es decir, un error u otro. Esta desviación de cada componente se calcula para los diferentes indicadores de calidad que existen, ρ = 1, 2, 4 ó 5. En la tabla 6.1 112 Algoritmo de localización para el Robot SILO6 Tabla 6.1: Desviación estándar de cada modo. Modo (ρ) σ1(m) σ2(m) 1 0.297 0.255 2 0.152 0.200 4 0.003 0.004 5 0.072 0.095 se presentan los resultados cuando el receptor permanece en un posición fija (condiciones estáticas). La matriz Cg se tiene que calcular para las condiciones dinámicas para las que se realizan los experimentos. En esta situación no interesa la desviación estándar, ya que se quiere conocer el error que introduce una medida del DGPS cuando la antena está en movimiento. Por lo tanto, la forma más precisa de describir este error consiste en tomar el rango en el que oscilan los datos obtenidos del DGPS en una posición fija. Con el EKF sólo se calculará la fase de actualización cuando el indicador de calidad o modo es el que ofrece mayor precisión, ρ = 4. En estos casos la matriz Cg queda como: Cg = ( 0.02 0 0 0.02 ) (6.14) Las expresiones de las actualizaciones de la posición y orientación, y de la matriz covariante del error usando la matriz de Kalman son (Gelb et al., 1986): X(k) = X(k, k − 1) + K(k) [Z(k, k − 1)− Z(k)] (6.15) P (k) = [I −K(k)H]P (k, k − 1) (6.16) donde I es la matriz identidad. Todas las ecuaciones obtenidas en este caṕıtulo derivan de las presentadas en la sección 5.3.2. 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 113 6.4. Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores Esta sección estudia el sistema sensorial utilizado en el robot SILO6 para llevar a cabo la tarea de localización. Además, se presentan las tareas realizadas para el estudio de la precisión, fuentes de error y la evolución del error de cada componente del sistema sensorial con el que se ha trabajado. También se presentan los experimentos y los resultados obtenidos de la implementación del algoritmo de localización basado en el EKF expuesto en este caṕıtulo. 6.4.1. Estudio del Sistema Sensorial El sistema sensorial utilizado en el robot SILO6 está compuesto de un DGPS (un receptor y una antena a bordo del robot), 18 encoders (situados en cada una de las tres articulaciones de las seis patas del robot), una brújula electromagnética y un inclinómetro. A continuación se describen las caracteŕısticas de cada sensor. 6.4.1.1. Sistema de Posicionamiento Global Diferencial Con el estudio de las caracteŕısticas del DGPS se pretende conocer la precisión que se puede conseguir en situaciones estáticas (antena DGPS en posición fija) y en situaciones dinámicas (antena DGPS en movimiento). Estas últimas son importantes para conocer la precisión que se puede alcanzar cuando el robot está en movimiento. Las tareas se realizan con el GPS en modo diferencial (DGPS) (véase el apéndice C). El equipo adquirido para este trabajo de tesis doctoral consta de un receptor GPS Trimble 5700 y una antena Zephyr (véase la Figura 6.13). Las caracteŕısticas comerciales del equipo son las siguientes: Precisión centimétrica en posicionamiento en tiempo real con datos RTK/OTF (Real Time Kinematic/On The Fly), actualizaciones de posición a 10 Hz y alrededor de 20 ms de latencia. Precisión submétrica en posicionamiento en tiempo real usando correcciones de pseudorango con menos de 20 ms de latencia. RTK extendido (eRTK). 114 Algoritmo de localización para el Robot SILO6 Procesador RTK de frecuencia dual adaptativa. Capacidad WAAS (Wide Area Augmentation System). Entrada de marcador de eventos dual. Puerto USB para transferencia de datos. Tarjeta compactFlash Tipo I para almacenamiento de datos. Carga interna de bateŕıa (no se requiere cargador externo de bateŕıa). 1PPS (One Pulse Per Second) de salida. OTF (On The Fly) automático, inicialización en movimiento. Tres puertos serie RS-232 para: • Salida NMEA (National Marine Electronics Association). • RTCM (Radio Technical Commission for Maritine Services) SC-104 entrada y salida. • Entrada y salida en Formato Trimble (CMR, Compact Measurement Record). Dos puertos TNC para: • Conectar a una antena GPS. • Conectar a una antena de radio. Las correcciones diferenciales se reciben desde la estación base que se encuentra en el Instituto de Automática Industrial (IAI-CSIC) a través de la red interna. Exiten tres formas de utilizar el DGPS según la señal diferencial recibida: 1. Omniestar (precisión por debajo del metro). 2. Señal de RNE (Radio Nacional de España), sólo en territorio español. 3. RTK (Real Time Kinematics) (precisión centimétrica). 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 115 Figura 6.13: Receptor GPS Trimble 5700 y antena Zephyr. En esta tesis doctoral se considera suficiente una precisión de ±0.02 m, por lo tanto se utiliza el método RTK. A continuación se presentan los resultados obtenidos en situaciones estáticas y dinámicas. Caracteŕısticas estáticas del DGPS Para estudiar la precisión en la medida y las caracteŕısticas del DGPS se han realizado pruebas tomando medidas con la antena situada en una posición fija. El modo de recibir las correcciones diferenciales se indica en la trama de datos obtenida del DGPS por la variable ρ (véase el apéndice C) y puede ser de tres tipos: correcciones diferencias fijas (ρ = 2), correcciones diferenciales recibidas por código (ρ = 4), y correcciones diferenciales recibidas por fase (ρ = 5). En las pruebas realizadas se constata que la mejor precisión se consigue cuando las correciones diferenciales se reciben por código desde la estación base. Todas las pruebas realizadas se han llevado a cabo utilizando este modo3. 3A partir de ahora se mencionará directamente modo ρ = 4 para hablar de este tipo de 116 Algoritmo de localización para el Robot SILO6 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 ∆ Coord. UTM Este (m) ∆ C oo rd . U T M N or te ( m ) Figura 6.14: Incrementos de los datos obtenidos del DGPS (∆Coordenadas UTM) en condiciones estáticas (puntos azules) y valor medio (punto rojo). La Figura 6.14 presenta los resultados de una prueba para describir la distribución de puntos obtenidos en las diferentes mediciones realizadas con el DGPS. Los valores representan incrementos de las coordenadas UTM (Universal Transverse Mercator) referidos a los mı́nimos valores obtenidos en cada coordenada para poder observar mejor el rango, o incremento, en el que oscilan los datos. El rango en el que se distribuyen las medidas de cada componente UTM (Este y Norte) está alrededor de los cuatro cent́ımetros. El valor medio obtenido para cada coordenada tiene un error de ±0.02 m aproximadamente. Se observa en la Figura 6.14 la nube de puntos alrededor del valor medio de la posición. La distribución de los valores de cada componente es una distribución gaussiana, lo que se puede comprobar en los histogramas que se presentan en la Figura 6.15 correcciones diferenciales 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 117 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0 10 20 30 40 50 60 70 80 90 100 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 0 10 20 30 40 50 60 (a) (b) Figura 6.15: Histogramas de las coordenadas UTM: (a) Este; (b) Norte. para cada coordenada UTM. Esto nos da la oportunidad de tomar la desviación estándar en cada componente como el error que se produce en la medida cuando se mide un punto fijo durante un determinado tiempo. En la tabla 6.2 se presentan los valores de la desviación estándar obtenida para cada componente en diferentes pruebas realizadas. Se han realizado 10 pruebas para analizar la desviación estándar de los datos recibidos en función de los diferentes modos de corrección diferencial (véase la tabla 6.2). Para cada prueba se ha incluido en la tabla los diferentes modos de corrección (proporcionados en la trama de datos) de los datos recibidos y se ha calculado la desviación estándar. Se observa que la menor desviación estándar se obtiene para las pruebas cuyos datos se recibieron exclusivamente en el modo ρ = 4. Además se ha constatado que las pruebas 3, 4 y 9, si se extraen los datos recibidos en modo ρ = 4, la desviación estándar es también menor que la global de la prueba. Los valores correspondientes al subconjunto ρ = 4 se indican con “*” (3∗, 4∗ y 9∗) Por otra parte, hay que resaltar la precisión de los valores medios obtenidos para cada componente cuando sólo se tienen en cuenta las medidas en modo ρ = 4. Todas las pruebas han sido realizadas con la antena situada en la misma posición; sin embargo, el tiempo de medición es diferente en cada prueba. Se puede observar que los valores medios en cada componente no se diferencian en más de un cent́ımetro, por lo tanto esta precisión entra dentro de los requisitos 118 Algoritmo de localización para el Robot SILO6 Tabla 6.2: Resultados de las medidas estáticas. Prueba UTM Este (m) σE (m) UTM Norte (m) σN (m) ρ 1 459059.704 0.002 4462629.818 0.005 4 2 459059.703 0.003 4462629.821 0.005 4 3 459059.706 0.008 4462629.820 0.008 4,5 3* 459059.703 0.003 4462629.820 0.005 4 4 459059.703 0.063 4462629.814 0.088 2,4,5 4* 459059.703 0.003 4462629.820 0.005 4 5 459059.705 0.004 4462629.822 0.004 4 6 459059.689 0.092 4462629.804 0.040 4,5 7 459059.697 0.057 4462629.817 0.091 2,4,5 8 459059.708 0.003 4462629.820 0.004 4 9 459059.700 0.044 4462629.821 0.024 2,4,5 9* 459059.702 0.003 4462629.823 0.004 4 10 459059.702 0.004 4462629.828 0.006 4 iniciales propuestos en el proyecto en el que se desarrolla esta tesis doctoral. La desviación estándar depende también de un parámetro de la trama de datos que proporciona el DGPS. Este parámetro se denonima HDOP y da información sobre la influencia que tiene sobre la medida la distribución geométrica de los satélites de los que se recibe señal. En la trama de datos obtenida del DGPS se indica por la variable ζ (véase el apéndice C). Cuanto mayor es el valor, peor es la distribución de los satélites y por lo tanto disminuye la precisión. Todas las medidas representadas en la tabla 6.2 se han tomado generalmente con un valor nulo del HDOP, ζ = 0. En la prueba que se representa en las Figuras 6.13 y 6.14 el HDOP toma los valores ζ = 1.8 y ζ = 1.9, por esta razón la desviación estándar obtenida para cada coordenada en este caso es algo mayor a las obtenidas en la tabla 6.2 en modo ρ = 4. La desviación estándar obtenida para cada coordenada es: σE = 0.004 m σN = 0.007 m. Generalmente, los valores obtenidos cuando 0 ≤ ζ < 1 son: σE = 0.003 m σN = 0.005 m. Caracteŕısticas dinámicas del DGPS Una vez estudiadas las caracteŕısticas del DGPS en condiciones estáticas, en 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 119 esta sección, se estudian las caracteŕısticas en condiciones dinámicas. Este estudio es importante para conocer cómo describe el DGPS las trayectorias seguidas por el robot. Hay que recordar que la precisión que se considera óptima para la tarea a realizar es ±0.02 m. El dispositivo utilizado para este estudio se muestra en la Figura 6.16. Consta de una gúıa lineal motorizada que arrastra a la antena a velocidad constante. Las pruebas se han realizado a las velocidades a las que generalmente se mueve el robot: 0.025 m/s y 0.050 m/s. El motor utilizado, controlado en velocidad, asegura el movimiento constante de la antena DGPS a lo largo de la trayectoria. Con motivo de analizar el comportamiento del DGPS en movimiento se han realizado diferentes experimentos. En la Figura 6.17 se muestran las posiciones obtenidas por el DGPS en una prueba donde se realiza el trayecto en los dos Figura 6.16: Dispositivo utilizado para el estudio del DPGS en condiciones dinámicas. 120 Algoritmo de localización para el Robot SILO6 0 0.5 1 1.5 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Coord. UTM Este (m) C oo rd . U T M N or te ( m ) 0 0.5 1 1.5 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Coord. UTM Este (m) C oo rd . U T M N or te ( m ) (a) (b) Figura 6.17: Trayectorias teórica de la antena (ĺınea roja) y descrita por el DGPS (ĺınea azul) en ambos sentidos: (a) Ida; (b) Vuelta. sentidos. Se representan los puntos obtenidos frente a los puntos teóricos, que son aquellos donde debeŕıa estar la antena en cada instante. Para calcular esta recta se toman los puntos fijos inicial y final de la trayectoria medidos con el DGPS durante un tiempo suficientemente amplio para conseguir una buena precisión. Por lo tanto, la recta calculada está afectada de un pequeño error, desviación estándar de la medida del punto inicial y final, que en las diferentes pruebas realizadas oscila entre 0.003 m y 0.006 m en la coordenada UTM Este, y entre 0.004 m y 0.008 m en la coordenada UTM Norte. A partir de la velocidad calculamos el número de puntos de la recta teórica. El estudio de estas trayectorias se centra en el análisis del error que se obtiene en la posición. Como error consideramos la diferencia entre la posición teórica y la obtenida con el DGPS en cada instante y se analiza tanto la componente longitudinal como la componente transversal a la trayectoria. Para llevar a cabo un control del seguimiento de trayectorias en el robot SILO6 interesa conocer el error que se produce en la coordenada transversal a la trayectoria que determina cuánto se desv́ıa el robot de la trayectoria a seguir. En cuanto al error en la coordenada longitudinal, su interés radica en conocer a lo largo de la trayectoria teórica lo que se está atrasando o adelantado el robot. Esto es importante en los puntos en los que el robot tiene que girar para seguir otra trayectoria. El error se define como el valor absoluto de la diferencia entre la posición teórica y la obtenida con el DGPS en cada instante para conocer la precisión en cada instante (véase la Figura 6.18). Los signos del error coinciden con los signos 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 121 0 20 40 60 80 100 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 Medidas E rr or ( m ) 0 20 40 60 80 100 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 Medidas E rr or ( m ) (a) (b) Figura 6.18: Error transversal (ĺınea azul) y error longitudinal (ĺınea roja) en valor absoluto en ambos sentidos: (a) Ida; (b) Vuelta. de las coordenadas x e y. El sistema de referencia se toma como el sistema de referencia del robot. En la Figura 6.18 se presentan ambos errores. Se puede apreciar que el error en la coordenada transversal está por debajo de los 0.02 m, por lo tanto dentro de la precisión que se desea. En cuanto al error longitudinal, sobrepasa en algunos instantes los 0.02 m. Aqúı tener en cuenta que este cálculo conlleva menos precisión debido a que ésta es la dirección del desplazamiento de la antena y en las posiciones teóricas calculadas hay una pequeña incertidumbre en esta dirección. Esta incertidumbre se debe a que para el cálculo de este error se debe hacer coincidir el mismo instante de tiempo de la medida recibida del DGPS y la posición teórica de la antena. Esto no sucede en el error transversal, ya que en este caso sólo interesa la distancia perpendicular de la posición obtenida del DGPS a la trayectoria teórica descrita por la antena, es decir, en este caso no interesa el instante. Conviene resaltar que para el seguimiento de trayectorias es más importante disminuir los errores transversales (el robot se aparta de la trayectoria) que los errores longitudinales (aunque el robot se aparta de punto en el que debeŕıa estar, al menos, queda en la trayectoria). El comportamiento de los errores es aleatorio, es decir, el signo tanto de los errores transversales como longitudinales cambia aleatoriamente. Este comportamiento es lógico debido a que el error en cada medida es independiente de la anterior. Esta caracteŕıstica es propia de la localización absoluta (véase 122 Algoritmo de localización para el Robot SILO6 la sección 4.2.4.2) en la cual se engloba el dispositvo DGPS. En cualquier caso si se constata que la precisión en la coordenada transversal al desplazamiento está dentro de los ±0.02 m requeridos y en el error longitudinal sobrepasa esta precisión en algunos instantes, pero generalmente también cumple los requisitos. En las pruebas realizadas tanto en condiciones estáticas como dinámicas se ha observado el buen comportamiento del DGPS y por lo tanto se ha mostrado el porqué de la elección para usarlo en el robot SILO6. 6.4.1.2. Odometŕıa En esta sección se describe el sistema odométrico utilizado sobre el robot SILO6. Se han realizado experimentos para estudiar las caracteŕısticas de este sistema, sobre todo interesa conocer el comportamiento de los errores introducidos en la posición ya que estos errores aumentan con la distancia recorrida. Es interesante conocer la precisión que se pueda alcanzar con la odometŕıa y cómo vaŕıa con la distancia recorrida porque en los casos en los que el DGPS no reciba señal de los satélites la estimación de la posición a partir de este método cobra una gran importancia. La odometŕıa es un método de localización utilizado en robots móviles para conocer la posición relativa del robot (Ojeda y Borenstein, 2004). Se basa en la información incremental proporcionada por sensores de desplazamiento (véase la sección 4.3.1.1). El sistema odométrico utilizado en el robot SILO6 se basa en encoders ópticos incrementales (1000− 2000 pulsos por vuelta). Cada una de las articulaciones del robot SILO6 dispone de un encoder. A partir de las lecturas de éstos se puede calcular el desplazamiento que tiene lugar respecto a la posición anterior. El principal problema de la odometŕıa es el aumento de los errores en la estimación de la posición con la distancia recorrida. Para disminuir estos errores es fundamental realizar una buena calibración geométrica del robot que consiste en definir con precisión el origen y orientación del sistema de referencia de cada pata respecto al sistema de referencia del robot. En la Figura 6.19 se observa el dispositivo utilizado para la calibración geométrica del robot. Las articulaciones 2 y 3 se calibran posicionándolas de manera que formen un ángulo de 90o entre śı. Tras calibrar el robot geométricamente, se asignan los ángulos de los motores de las articulaciones respecto al sistema de referencia de cada pata. En los experimentos realizados para el estudio del error introducido por la odometŕıa en la estimación de la posición, el robot sigue una trayectoria rectiĺınea 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 123 1 2 3 Articulaciones Figura 6.19: Dispositivo para la calibración geométrica del robot SILO6. en lazo abierto. Interesa conocer la distancia que se puede recorrer bajo una precisión de ±0.02 m utilizando este método. En estos experimentos no se utiliza un control del seguimiento de trayectorias. Como resumen de la experimentación realizada se presentan los resultados de uno de estos experimentos, al que denominaremos experimento EXP-1. Debido a las holguras en las articulaciones, flexiones en los eslabones, deslizamientos en los pies, etc. el robot tiende a desplazarse y girar hacia un lado y no seguir la ĺınea recta. Ésto se puede observar en la Figura 6.20 donde se representan los puntos de parada del robot y la posición estimada a partir de la odometŕıa. El robot realiza paradas intermedias durante el desplazamiento para poder medir externamente su posición y aśı conocer el error que se está introduciendo en ese instante con la odometŕıa. Las puntos de parada del robot no son equidistantes ya que siempre se tienen que producir cuando el robot tiene las seis patas apoyadas en el suelo. Se ha utilizado el DGPS como sistema externo para medir las posiciones donde el robot se para. Midiendo con este dispositivo durante un tiempo, en nuestro caso unos minutos es suficiente, se consigue una gran precisión en la posición con un error (desviación estándar de la distribución gaussiana obtenida) que, generalmente, no supera los ±0.004 124 Algoritmo de localización para el Robot SILO6 m. Estas posiciones (coordenadas X e Y ) se presentan en la tabla 6.3. Como se ha dicho anteriormente, con la odometŕıa se obtienen incrementos de la posición respecto al sistema de referencia del robot en el instante de la medida. En cambio, los valores obtenidos son referidos siempre al sistema de referencia original, es decir, el sistema de referencia del cuerpo del robot cuando comienza el desplazamiento, debido a que no conocemos la orientación del cuerpo y por lo tanto la del sistema de referencia del robot en cada instante. Estos incrementos se tienen que calcular en el sistema de referencia absoluto para poder compararlos con los obtenidos en cada punto de parada. Se aplica una traslación y rotación para realizar este cambio de sistema. Con este método se introducen errores significativos en la coordenada transversal al movimiento del robot ya que no se conoce la orientación del sistema de referencia del robot en cada instante. En la Figura 6.20 se representan, con puntos en rojo, las posiciones estimadas con la odometŕıa en cada punto de parada. Se aprecia un aumento del error de la estimación de la posición con la odometŕıa con la distancia recorrida. Nótese que la Figura 6.20 está representada en el sistema de referencia absoluto y que el robot se mueve en el sentido negativo de la coordenada X. En este caso consideramos el error de la estimación como la diferencia entre las posiciones de los puntos de parada realizados (puntos negros) y las posiciones estimadas con la odometŕıa en estos puntos de parada (puntos rojos) indicados en la Figura 6.20. También se observa que la odometŕıa describe la trayectoria del robot como una recta mientras el robot se ha desplazado hacia su derecha (véase Xo y Yo en la tabla 6.3), lo cual confirma que el error en la coordenada transversal al desplazamiento es significativo. Hay que diferenciar entre la coordenada transversal al moviento del cuerpo (respecto al sistema de referencia del cuerpo del robot es y’ en la Figura 4.1) y las coordenadas absolutas X e Y (x e y en la Figura 4.1). En algunos experimentos pueden coincidir las direcciones de las coordenadas de ambos sistemas de referencia. Es importante conocer qué error se introduce en cada instante con la odometŕıa y la evolución de dicho error para saber a partir de qué distancia recorrida la precisión es menor que la requerida. La Figura 6.21 describe el error relativo a la distancia recorrida por el robot para cada coordenada , ∆x y ∆y, y el error relativo absoluto, ∆, diferencia entre la posición del robot y la posición estimada con la odometŕıa. Se observa que el error relativo absoluto va aumentando a medida que el robot se desplaza. El error relativo a la distancia recorrida en cada coordenada y el error relativo absoluto en cada punto de parada del robot se presentan en la tabla 6.3. 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 125 0 1 2 3 4 5 6 7 8 −4 −3.5 −3 −2.5 X (m) Y ( m ) Figura 6.20: Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con la odometŕıa (puntos azules) y posiciones estimadas en los puntos de parada con la odometŕıa (puntos rojos). Tabla 6.3: Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con odometŕıa (Xo y Yo) y errores relativos a la distancia ∆x, ∆y y ∆. P X (m) Y (m) Xo(m) Yo(m) ∆x(m) ∆y(m) ∆(m) 1 7.9033 -3.5103 7.9033 -3.5103 0 0 0 2 7.6074 -3.5206 7.6036 -3.5082 0.0038 -0.0124 0.0129 3 7.1500 -3.5348 7.1538 -3.5051 0.0062 -0.0297 0.0303 4 6.7225 -3.5479 6.7039 -3.5020 0.0185 -0.0459 0.0495 5 6.2920 -3.5536 6.2541 -3.4989 0.0379 -0.0547 0.0665 6 5.8477 -3.5508 5.8043 -3.4958 0.0434 -0.0550 0.0700 7 5.3999 -3.5345 5.3419 -3.4926 0.0580 -0.0419 0.0715 8 4.9852 -3.5254 4.9045 -3.4896 0.0807 -0.0357 0.0882 9 4.5292 -3.5238 4.4423 -3.4865 0.0869 -0.0374 0.0946 10 4.1050 -3.4890 3.9924 -3.4834 0.1126 -0.0056 0.1127 11 3.6844 -3.4635 3.5426 -3.4803 0.1418 0.0167 0.1428 12 3.2815 -3.4198 3.1177 -3.4773 0.1638 0.0575 0.1736 13 2.8541 -3.3712 2.6679 -3.4742 0.1862 0.1031 0.2128 14 2.4442 -3.3212 2.2306 -3.4712 0.2136 0.1500 0.2610 15 2.0149 -3.2634 1.7806 -3.4681 0.2343 0.2047 0.3111 16 1.6048 -3.1883 1.3432 -3.4652 0.2616 0.2769 0.3809 17 1.0183 -3.0710 0.6936 -3.4606 0.3247 0.3897 0.5072 Obviamente, se aprecia el aumento del error relativo con la distancia recorrida. A partir de estos datos se puede calcular aproximadamente la distancia a la que el error en la estimación de la posición supera los ±0.02 m requeridos. Esta distancia 126 Algoritmo de localización para el Robot SILO6 0 2 4 6 8 10 12 14 16 18 −0.2 0 0.2 0.4 0.6 0.8 1 1.2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a Figura 6.21: Errores relativos a la distancia recorrida introducidos por la odometŕıa: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo) se calcula a partir del error relativo absoluto medio introducido entre dos puntos de parada consecutivos del robot, que denominaremos factor de error relativo de odometŕıa δo. Teniendo en cuenta los resultados obtenidos en el experimento EXP-1 se ha obtenido que δo = 0.06609 . A partir de este factor se puede aproximar la distancia, do, que el robot puede recorrer estimando la posición con la odometŕıa y estando la precisión por debajo de ±0.02 m. Esta distancia es: do = 0.02 m δo = 0.3026 m (6.17) Estimando la posición con la odometŕıa, ésta es la distancia que el robot puede desplazarse mantiendo la precisión requerida en esta aplicación. Hay que resaltar que do está influenciado por las caracteŕısticas del robot- entorno, es decir, la distancia puede variar al cambiar el factor de error relativo de odometŕıa, δo. En los experimentos realizados las caracteŕısticas del robot- entorno (holguras, flexiones y deslizamientos) tienen una gran influencia. En el caso de las holguras pueden provocar desplazamientos no deseados del robot. Por 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 127 lo tanto las caracteŕısticas del robot-entorno son importantes para estimar los datos anteriores. 6.4.1.3. Brújula electromagnética En esta sección se presentan las caracteŕısticas de la brújula electromagnética utilizada para conocer la orientación del cuerpo del robot y el método seguido para su calibración. Se ha utilizado una brújula electromagnética denominada 1525 Analog Sensor. El ángulo (orientación del cuerpo del robot respecto al norte magnético) obtenido se utiliza para conocer en todo momento la orientación del cuerpo del robot y también para estimar mejor los incrementos de la posición mediante técnicas dead-reckoning (véase la sección 6.4.1.7). Las caracteŕısticas comerciales de esta brújula electromagnética son: Alimentación: 5 V CC. Corriente: 0.018 a 0.019 Ap. Las salidas se asemejan a curvas senoidales que se cruzan aproximadamente en 2.5 V, tienen el máximo en 2.88 V y el mı́nimo en 2.12 V. A estas señales las denominaremos señal seno y señal coseno. El sensor opera en posición vertical y está diseñado para medir la dirección de la componente horizontal del campo terrestre (componente de la brújula electromagnética). Si el sensor se aparta de la vertical, queda afectado por la componente vertical del campo terrestre que puede introducir algún error. En la práctica las medidas son aceptables hasta con 12o de inclinación, aproximadamente. Los ĺımites de la temperatura de operación son de -20o a 85o C. El sensor pesa aproximadamente 2.25 g. En la Figura 6.22(a) se muestra la forma y dimensiones de la brújula electromagnética. Calibración de la Brújula Electromagnética. La brújula electromagnética está situada en el robot a una determinada altura sobre el cuerpo (0.065 m) para evitar las interferencias producidas por los sistemas 128 Algoritmo de localización para el Robot SILO6 Brújula electromagnética (a) (b) Figura 6.22: (a) Brújula electromagnética: 1525 Analog Sensor; (b) Posición de la brújula electromagnética en el robot SILO6. activos (motores) y sujeta firmemente a una barra para minimizar vibraciones al desplazarse el robot (véase la Figura 6.22(b)). Las pruebas realizadas para calibrar y determinar las caracteŕısticas de la brújula electromagnética han consistido en girar el robot un ángulo 2π radianes (sentido antihorario) parando en distintos ángulos. El dispositivo utilizado en el robot SILO6 se puede observar en la Figura 6.23. El procedimiento es el siguiente: en cada punto de parada se toma nota de la salidas que se obtienen con la brújula electromagnética y la posición de la antena DGPS. Los datos de la brújula electromagnética se toman cada cien milisegundos durante veinte segundos. En el caso del DGPS, los datos se toman cada segundo durante dos minutos. En ambos casos la medición se realiza durante un intervalo de tiempo para obtener un valor más preciso ya que ambos tipos de datos tienen una distribución gaussiana, por lo tanto se toma la media de las medidas obtenidas con cada sensor. Con el DGPS se calcula la orientación a partir de la recta formada por dos puntos: por un lado, el origen de la circunferencia descrita para la calibración (véanse la Figura 6.23 y la Figura 6.24); y por otro lado, la posición de la antena en 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 129 Antena DGPS Brújula Electromagnética Soporte Origen Circunferencia Figura 6.23: Dispositivo utilizado para la calibración de la brújula electromagnética. cada punto de parada (véase la Figura 6.24). A partir de esta recta se obtiene con el DGPS la orientación del punto de parada respecto a la coordenada UTM Norte. La razón de esta elección para llevar a cabo la calibración se debe a que al calibrar directamente con el norte del DGPS se evita tener en cuenta la declinación magnética (diferencia entre el norte geográfico y norte magnético) y la declinación que existe entre el norte geográfico y el norte de cada zona UTM. De esta forma el ángulo obtenido con la brújula electromagnética está referido al mismo sistema de referencia que utiliza el DGPS. En la Figura 6.24 se representa el origen de la circunferencia (en rojo) y las posiciones de la antena en la que se han tomado los valores de la brújula electromagnética (en azul). Estos datos se consideran suficientes para la calibración. Las salidas de la brújula electromagnética se pueden observar en la Figura 6.25 y los datos se presentan en la tabla 6.4. Se aprecia la forma de la curva seno y coseno en cada salida. El ángulo se calcula a partir de la siguiente expresión: α = arctan( Vseno − Vmedioseno rangoseno , Vcoseno − Vmediocoseno rangocoseno ) (6.18) donde Vseno y Vcoseno son los valores medidos, Vmedioseno = 2.488987 V y 130 Algoritmo de localización para el Robot SILO6 4.6256 4.6256 4.6256 4.6256 4.6256 4.6256 4.6256 4.6256 x 10 5 4.0976 4.0977 4.0977 4.0978 4.0979 4.0979 4.098 x 10 4 X (m) Y ( m ) Figura 6.24: Posiciones utilizadas para calibrar la brújula electromagnética: puntos de parada realizados (azul), origen de la circunferencia (rojo). Vmediocoseno = 2.471974 V son los valores medios de las salidas seno y coseno obtenidos, rangoseno = 1.371311 V y rangocoseno = 1.437199 V son los rangos en los que se mueven los datos de las salidas seno y coseno. Los ángulos obtenidos en cada posición con la brújula electromagnética y el DGPS se representan en la tabla 6.4 y se indican en la Figura 6.26. Los datos recogidos son los representados con puntos azules. Se ha aplicado una traslación a uno de los dos segmentos (puntos rojos) para alinear todos los puntos en una recta (véase la Figura 6.26). El desalineamiento se produce en el cambio de signo de la salida seno. Esto se produce cuando el ángulo con el DGPS es 44.9370o. La traslación ha consistido en sumar 2π a los valores de la brújula electromagnética obtenidos entre 0o y 44.9370o y los valores negativos (−π y 0o). Para realizar una calibración más precisa, el dominio de la función B se dividen en 6 zonas y cada zona se aproxima por una recta. Las zonas definidas son las siguientes: 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 131 Tabla 6.4: Datos obtenidos en cada punto de parada. Medidas Seno (V) Coseno (V) Brújula (rad) DGPS (rad) 1 2.0673 1.8564 -2.5189 0.1140 2 2.2537 1.7458 -2.8142 0.4049 3 2.3692 1.7101 -2.9783 0.5679 4 2.4632 1.7031 -3.1064 0.7018 5 2.5511 1.7048 3.0570 0.8345 6 2.5992 2.5992 2.9854 0.9340 7 2.6980 1.7774 2.8360 1.0955 8 2.8070 1.8519 2.6484 1.2966 9 2.9342 1.9488 2.4133 1.5111 10 2.9837 2.0242 2.2831 1.6425 11 3.0324 2.1503 2.0850 1.8251 12 3.0834 2.2808 1.8686 2.0226 13 3.1030 2.3992 1.6833 2.1882 14 3.1126 2.4863 1.5489 2.3204 15 3.1101 2.5807 1.4053 2.4560 16 3.0989 2.6786 1.2582 2.6120 17 3.0748 2.7786 1.1076 2.7926 18 3.0504 2.8387 1.0135 2.8997 19 3.0118 2.9011 0.9064 3.0266 20 2.9739 2.9481 0.8180 3.1335 21 2.9259 2.9994 0.7150 3.2452 22 2.8547 3.0526 0.5835 3.3988 23 2.7762 3.0912 0.4524 3.5473 24 2.6924 3.1222 0.3169 3.6958 25 2.6159 3.1345 0.1981 3.8257 26 2.5131 3.1356 0.0380 3.9878 27 2.3924 3.1403 -0.1503 4.1355 28 2.2978 3.1260 -0.2972 4.2688 29 2.1876 3.0828 -0.4772 4.4328 30 2.0684 3.0183 -0.6788 4.6087 31 1.9328 2.9135 -0.9225 4.8423 32 1.8478 2.8172 -1.0962 5.0155 33 1.8027 2.7443 -1.2089 5.1391 34 1.7629 2.6484 -1.3430 5.2804 35 1.7421 2.5174 -1.5129 5.4542 36 1.7413 2.4159 -1.6422 5.5925 37 1.7803 2.2608 -1.8478 5.7838 38 1.8323 2.1522 -2.0057 5.9205 39 1.9028 2.0391 -2.1846 6.0925 40 1.9560 1.9647 -2.3081 6.2005 41 2.0449 1.8709 -2.4827 0.0827 132 Algoritmo de localización para el Robot SILO6 0 5 10 15 20 25 30 35 40 45 1.6 1.8 2 2.2 2.4 2.6 2.8 3 3.2 Medidas V ol ta je ( V ) Figura 6.25: Salidas del seno y coseno obtenidas de la brújula electromagnética. Zona 1 : α ≥ 362.1796o Zona 2 : 254.1315o ≤ α < 362.1796o Zona 3 : 198.7665o ≤ α < 254.1315o Zona 4 : 151.7435o ≤ α < 198.7665o Zona 5 : 88.7437o ≤ α < 151.7435o Zona 6 : α < 88.7437o (6.19) Con esta calibración se obtiene una buena precisión, siendo la diferencia mayor entre el ángulo calculado con el DGPS y el obtenido a partir de la brújula electromagnética calibrada de 1.076072o. 6.4.1.4. Dead-reckoning En esta sección se estudian las técnicas dead-reckoning (véase la sección 4.3.1.2) y se presentan los experimentos realizados para estimar la posición y orientación del robot con estas técnicas. Los sensores utilizados son 18 encoders y una brújula electromagnética. En este caso, a diferencia de la odometŕıa, también se puede estimar la orientación del robot a partir de la brújula electromagnética. 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 133 −4 −3 −2 −1 0 1 2 3 4 −4 −2 0 2 4 6 8 Orientacion con DGPS (rad) O rie nt ac io n co n B rú ju la e le ct ro m ag né tic a (r ad ) Figura 6.26: Representación de los ángulos obtenidos con la brújula electromagnética y el DGPS en cada punto de parada: los datos recogidos en azul y los datos a los que se ha aplicado la traslación en rojo. Los experimentos realizados están orientados a estudiar el error introducido por dead-reckoning y la evolución de éste. Se compara también con los resultados obtenidos con odometŕıa para apreciar la disminución del error que se produce en la estimación de la posición con dead-reckoning. Es obvio que esta estimación mejore con estas técnicas ya que, al conocer la orientación del robot en cada instante, se obtiene la orientación del sistema de referencia del robot. El conocimiento de la orientación de este sistema, a priori, implica más precisión en los incrementos de posición calculados a partir del modelo cinemático del robot por dead-reckoning que los calculados mediante la odometŕıa. El estudio del error introducido por dead-reckoning es de vital importancia para la fase de estimación del filtro de Kalman extendido expuesto en la sección 6.2. Para analizar el comportamiento de las técnicas dead-reckoning se han realizado diferentes experimentos. En esta sección presentamos el mismo experimento EXP-1 (presentado para el estudio de la odometŕıa) para explicar los resultados obtenidos y aśı comparar los resultados. Por lo tanto, el robot sigue 134 Algoritmo de localización para el Robot SILO6 0 1 2 3 4 5 6 7 8 −4 −3.5 −3 −2.5 X (m) Y ( m ) Figura 6.27: Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con dead-reckoning (puntos azules) y posiciones estimadas en los puntos de parada con dead-reckoning (puntos rojos). de nuevo una trayectoria rectiĺınea en lazo abierto. También se quiere conocer la distancia que el robot puede desplazarse bajo una precisión de ±0.02 m con estas técnicas. Los experimentos se realizan de la misma forma que los de la odometŕıa, parando el robot cada cierto tiempo y comparando en ese instante la estimación de la posición con el valor de la posición calculado con el sistema externo que, como hemos mencionado anteriormente, es el DGPS. En la Figura 6.27 se presenta la trayectoria estimada con dead-reckoning frente a las posiciones del robot medidas con el sistema externo. Nuevamente, como en el caso de la odometŕıa, los principales fuentes de error que aparecen en estas técnicas son las holguras, flexiones y delizamientos, principalmente. Con dead-reckoning la estimación de la posición del robot mejora considerablemente frente a la odometŕıa (véase la Figura 6.28). El valor de las posiciones en cada punto de parada y el error se detallan en la tabla 6.5. La mejor estimación de la posición que se obtiene con dead-reckoning se debe a que se conoce la orientación del robot en cada instante. Aśı, se pueden calcular los incrementos de la posición obtenidos a partir de los encoders con más precisión. En el caso de la odometŕıa la trayectoria estimada era casi una ĺınea recta ya que no hab́ıa conocimiento de los cambios de orientación del robot. Ahora se detectan estos cambios y los incrementos de posición describen mejor esta trayectoria. Tanto en la Figura 6.27 como en la tabla 6.5 se observa cómo se aproxima la posición estimada mejor a la real con dead-reckoning. El error relativo a la distancia recorrida, por supuesto, disminuye respecto a la odometŕıa (véase la Figura 6.29). En concreto, en el experimento EXP-1, esta disminución del error se percibe en la coordenada Y, ya que en este caso coincide 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 135 Tabla 6.5: Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con dead- reckoning (Xdr y Ydr) y errores relativos a la distancia ∆x, ∆y y ∆. P X(m) Y(m) Xdr(m) Ydr(m) ∆x(m) ∆y(m) ∆(m) 1 7.9033 -3.5103 7.9033 -3.5103 0 0 0 2 7.6074 -3.5206 7.6036 -3.5118 0.0038 -0.0088 0.0096 3 7.1500 -3.5348 7.1538 -3.5152 0.0062 -0.0196 0.0206 4 6.7225 -3.5479 6.7040 -3.5182 0.0185 -0.0297 0.0350 5 6.2920 -3.5536 6.2541 -3.5187 0.0379 -0.0348 0.0515 6 5.8477 -3.5508 5.8043 -3.5124 0.0434 -0.0383 0.0579 7 5.3999 -3.5345 5.3422 -3.4989 0.0577 -0.0356 0.0678 8 4.9852 -3.5254 4.9049 -3.4903 0.0803 -0.0351 0.0876 9 4.5292 -3.5238 4.4427 -3.4791 0.0864 -0.0448 0.0973 10 4.1050 -3.4890 3.9934 -3.4596 0.1116 -0.0294 0.1154 11 3.6844 -3.4635 3.5442 -3.4347 0.1402 -0.0287 0.1431 12 3.2815 -3.4198 3.1204 -3.4040 0.1611 -0.0157 0.1618 13 2.8541 -3.3712 2.6720 -3.3697 0.1821 -0.0015 0.1821 14 2.4442 -3.3212 2.2366 -3.3294 0.2076 0.0082 0.2078 15 2.0149 -3.2634 1.7903 -3.2721 0.2246 0.0087 0.2248 16 1.6048 -3.1883 1.3575 -3.2087 0.2473 0.0204 0.2481 17 1.0183 -3.0710 0.7188 -3.0930 0.2995 0.022 0.3003 más o menos con la transversal del desplazamiento del robot. En esta componente es donde mejor se aprecia la disminución del error relativo al tener en cuenta las 0 1 2 3 4 5 6 7 8 −4 −3.5 −3 −2.5 X (m) Y ( m ) Figura 6.28: Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con dead-reckoning (puntos azules) y trayectoria estimada con odometŕıa (puntos rojos). 136 Algoritmo de localización para el Robot SILO6 0 2 4 6 8 10 12 14 16 18 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a Figura 6.29: Errores relativos a la distancia recorrida introducidos por dead-reckoning: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). medidas de la brújula electromagnética. El error de la coordenada X es parecido al obtenido con la odometŕıa ya que esta coordenada coincide más o menos con la dirección del desplazamiento del robot. El estudio realizado se ha centrado, sobre todo, en conocer la distancia que se puede recorrer estimando la posición del robot con dead-reckoning. Este análisis es importante porque con esta técnica se estima la posición del robot en la fase de estimación del EKF, presentado en la sección 6.2. También cobra un papel importante en los casos en los que la precisión del DGPS disminuye debido a las diferentes fuentes de error que se pueden presentar en este dispositivo (véase el apéndice C). Por esta razón, mejorar la precisión de las técnicas dead-reckoning para robots caminantes es una de las tareas primordiales en esta tesis doctoral. En la tabla 6.5 se presentan los errores relativos ∆x, ∆y y ∆. Al igual que en la odometŕıa, a partir de estos datos se puede observar aproximadamente a qué distancia el error en la estimación de la posición supera los ±0.02 m especificados en nuestros requerimientos. Esta distancia se calcula a partir del error relativo absoluto medio introducido que denominaremos factor de error relativo de dead-reckoning δdr. Teniendo en cuenta los experimentos realizados se 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 137 obtiene el valor δdr = 0.04. A partir de este factor se puede aproximar la distancia, ddr, que el robot puede recorrer estimando la posición con dead-reckoning y estando la precisión por debajo de ±0.02 m, es decir: ddr = 0.02 m δdr = 0.500 m (6.20) Nótese que este valor para el caso de la odometŕıa es do = 0.3026 m. Por lo tanto, utilizando dead-reckoning se puede recorrer más distancia satisfaciendo la precisión requerida. Por lo tanto a mayor distancia, más tiempo se tiene para recuperar los problemas que pueda presentar el DGPS en determinados momentos. Por último, hay que resaltar lo menciado también en el caso de la odometŕıa respecto al estado del robot. Este estado influye en las estimaciones de la posición obtenidas en cada experimento al variar las holguras existentes en las patas, deslizamientos, etc. 6.4.2. Fusión Sensorial: EKF En esta sección se describe la implementación sobre el robot SILO6 del filtro de Kalman extendido (EKF), presentado anteriormente en este caṕıtulo. El algoritmo se implementa en Lenguaje C utilizando el sistema operativo QNX4. Un experimento realizado y los resultados obtenidos se presentan en esta sección. Además, se presentan los resultados obtenidos en el mismo experimento al considerar los datos del inclinómetro, concretamente el ángulo de balanceo del cuerpo del robot, para estimar los desplazamientos transversales que tienen lugar en el robot debido a las holguras. Los resultados de todas las estimaciones de la posición se refieren al mismo experimento, que denominaremos experimento EXP-2, para poder compararlos y analizar el mejor comportamiento de los métodos utilizados. En el experimento EXP-2, al igual que el EXP-1 mostrado en la sección 6.4.1.2 y 6.4.1.4, el robot debe seguir una trayectoria rectiĺınea. En principio, el robot tendŕıa que seguir una trayectoria paralela al eje longitudinal del sistema de referencia del robot, pero debido a las imperfecciones del mecanismo, el robot tiende a apartarse de la trayectoria (véase la Figura 6.30). La velocidad deseada a la que se desplaza el robot es a 0.04 m/s. El centro geométrico del robot en el instante inicial se toma como origen. 4QNX es un sistema operativo de tipo UNIX y presenta como caracteŕısticas más importantes que es un sistema operativo multitarea y de tiempo real. 138 Algoritmo de localización para el Robot SILO6 Figura 6.30: Trayectoria deseada (ĺınea roja), trayectoria descrita (ĺınea negra) y posición final del robot SILO6 para el experimento EXP-2. 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 139 6.4.2.1. EKF con encoders, brújula electromagnética y DGPS La implementación del EKF que se estudia en esta sección se compone de la información proporcionada por los encoders, una brújula electromagnética y un DGPS. Por un lado, en la fase de estimación se utilizan técnicas dead-reckoning (encoders y brújula electromagnética) para estimar la posición del robot; por otro lado, en la fase de actualización se estima la posición a partir del DGPS. En el experimento EXP-2 se calculan las posiciones estimadas del robot a partir de la odometŕıa, dead-reckoning, DGPS y el EKF. En esta sección, el estudio se centra en los resultados obtenidos con el EKF, pero también se muestran los resultados de los otros tres métodos para analizar las mejoras que introduce el EKF. La principal razón del estudio de la localización del robot, además de la importancia que tiene en la tarea que realiza el robot SILO6, se debe al papel que juega en el control del seguimiento de la trayectoria del robot, que se expone en el caṕıtulo 7. En la Figura 6.31 se observan las posiciones estimadas con el EKF en el experimento EXP-2. La trayectoria estimada está muy próxima a la que sigue el robot. Las posiciones obtenidas con el sistema externo (DGPS) del robot SILO6 en cada punto de parada, las estimadas por el EKF y los errores relativos a la distancia recorrida se presentan en la tabla 6.6. La mejor aproximación en la estimación de la posición que se introduce con el EKF es evidente. Esto se puede apreciar en la Figura 6.32. Aqúı se observan todas las estimaciones obtenidas con los diferenes métodos utililizados: odometŕıa, dead-reckoninig (encoders y brújula electromagnética), DGPS y EKF. La estimación de la posición con odometŕıa vuelve a ser la más imprecisa, como es de esperar ya que no se estiman los cambios de orientación del robot. Con dead-reckoning se mejora la estimación respecto a la odometŕıa. En este caso se detecta el cambio de orientación pero esta estimación es solo suficiciente para una distancia corta, como vimos en la sección 6.4.1.4. Las medidas del DGPS se obtienen en modo ρ = 4 durante el experimento EXP-2, por este motivo con este dispositivo se estima la trayectoria del robot con un error aceptable, dentro de la precisión requerida. En los casos en que la corrección diferencial no se reciba en este modo no se tiene en cuenta la información del DGPS ya que el error que introduce es mucho mayor que el requerido en este trabajo. Cuando esto sucede, sólo se tiene en cuenta dead-reckoning para estimar la posición del robot, por lo tanto solo se realiza la fase de estimación del EKF. 140 Algoritmo de localización para el Robot SILO6 Las ventajas del EKF implementado son diversas. Por un lado suaviza la trayectoria estimada al tener en cuenta dead-reckoning, y por otro lado, con el DGPS, se consigue una precisión aceptable. El error de las medidas de dead- reckoning y del DGPS son mayores que en el caso del EKF. Precisamente una de las ventajas del EKF es que se minimiza el error de los sensores utilizados. En la Figura 6.33 se representa el error relativo a la distancia obtenido con el EKF. Comparando con dead-reckoning, este error es menor. Salvo en algunos puntos de parada, la diferencia entre las posiciones está dentro del rango que requerimos (véase la tabla 6.6). Hay que tener en cuenta que al medir la posición del robot en un punto de parada con el DGPS y la posición estimada con otro método, hay una diferencia de tiempo que provoca una alteración en los errores calculados en las tablas 6.3, 6.5 y 6.6. La información de los encoders, brújula electromagnética y DGPS se toma cada segundo. Por lo tanto, al emitir el comando para detener el robot cabe la posibilidad de que el robot se haya desplazado una distancia desde la última vez que se tomaron las medidas. Esta distancia dependerá de la velocidad a la que se desplaza el robot. En nuestro caso es de 0.04 m/s, esto quiere decir que el robot se puede desplazar hasta 0.04 m en el intervalo de tiempo entre una medida y otra. Debido a esto, la diferencia entre la posición del robot medida con el sistema externo y la estimada con el EKF −23.5 −23 −22.5 −22 −21.5 −21 −20.5 −20 −19.5 −19 −18.5 −18 −6 −5.5 −5 −4.5 −4 X (m) Y ( m ) Figura 6.31: Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con EKF (puntos azules) y posiciones estimadas en los puntos de parada con EKF (puntos rojos). 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 141 Tabla 6.6: Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con EKF (Xekf y Yekf ) y errores relativos a la distancia ∆x, ∆y y ∆. P X(m) Y(m) Xekf (m) Yekf (m) ∆x(m) ∆y(m) ∆(m) 1 -18.2976 -5.6453 -18.3121 -5.6474 0.0146 0.0021 0.0147 2 -18.6303 -5.6480 -18.6244 -5.6464 -0.0059 -0.0017 0.0061 3 -19.0139 -5.6650 -19.0197 -5.6622 0.0058 -0.0028 0.0064 4 -19.4128 -5.6396 -19.4079 -5.6395 0.0049 -0.0001 0.0049 5 -19.7788 -5.5852 -19.7633 -5.6051 0.0155 0.0200 0.0252 6 -20.1872 -5.5493 -20.1602 -5.5514 -0.0270 0.0022 0.0271 7 -20.5490 -5.4988 -20.5330 -5.5011 -0.0160 0.0023 0.0162 8 -20.9341 -5.4467 -20.9289 -5.4516 -0.0052 0.0048 0.0072 9 -21.3059 -5.3567 -21.2944 -5.3559 -0.0115 -0.0008 0.0116 10 -21.6528 -5.2509 -21.6328 -5.2359 -0.0200 -0.0151 0.0250 11 -22.0263 -5.0923 -22.0192 -5.1079 -0.0072 0.0156 0.0172 12 -22.3936 -4.9695 -22.4022 -4.9645 0.0086 -0.0050 0.0100 13 -22.7316 -4.8027 -22.7328 -4.7976 0.0012 -0.0051 0.0052 14 -23.0827 -4.6214 -23.0399 -4.6357 -0.0428 0.0143 0.0451 −23.5 −23 −22.5 −22 −21.5 −21 −20.5 −20 −19.5 −19 −18.5 −18 −6.5 −6 −5.5 −5 −4.5 −4 X (m) Y ( m ) Figura 6.32: Puntos de parada del robot (puntos negros) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning (azul claro), DGPS (verde) y EKF (rojo). 142 Algoritmo de localización para el Robot SILO6 0 2 4 6 8 10 12 14 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a Figura 6.33: Errores relativos a la distancia recorrida introducidos por EKF: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). puede superar en algunos puntos de parada los cuatro cent́ımetros. Ésto es más evidente en el punto final del desplazamiento del robot. Respecto al error de los cálculos realizados con el EKF para obtener una estimación de la posición del robot, hay que resaltar que el error de estos cálculos es menor que el error introducido con dead-reckoning y DGPS. Ésta es una de las ventajas del EKF, minimizar el error de la estimación realizada. Del mismo modo que el error con dead-reckoning viene dado en el EKF por la matriz Co y el error del DGPS por la matriz Cg, la matriz que describe el error del EKF para las tres componentes en cada instante es la matriz P (k). En la Figura 6.34 se muestra la evolución del error en la medida del EKF para las coordenadas de posición, X1 y X2, y la coordenada de orientación, X3. Se observa cómo este error disminuye y resulta ser menor que el introducido por dead-reckoning y el DGPS. Por lo tanto, la medida calculada con el EKF es más precisa que las obtenidas con dead-reckoning y DGPS (véase la Figura 6.34). 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 143 0 20 40 60 80 100 120 140 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 Medidas E rr or ( m ) Figura 6.34: Errores en la medida calculada con EKF: para la coordenada X1 (azul), para la coordenada X2 (rojo) y para la coordenada X3 (verde). 6.4.2.2. EKF con encoders, brújula electromagnética, inclinómetro y DGPS El EKF considerado en esta sección cuenta con un inclinómetro, además de los encoders, brújula electromagnética y DGPS. Como se expuso en la sección 6.2.2, se ha implementado un método a partir de la información proporcionada por el inclinómetro para estimar los desplazamientos transversales que tienen lugar en el robot debido a las holguras que aparecen en las diferentes articulaciones. Concretamente, de la información que se obtiene del inclinómetro se considera el ángulo de balanceo. Los datos se han tomado del experimento EXP-2, representado en la Figura 6.30. Las posiciones estimadas con el EKF se presentan en la Figura 6.35. Nuevamente, el EKF describe bien la trayectoria seguida por el robot. Las posiciones de cada punto de parada del robot, las estimaciones de las posiciones mediante el EKF y los errores se detallan en la tabla 6.7. En la sección 6.4.2.1 se han presentado los resultados sin tener en cuenta el ángulo de balanceo. Ahora, considerando este ángulo, se espera que la estimación de la posición del robot empleando técnicas dead-reckoning mejore. Esto se puede 144 Algoritmo de localización para el Robot SILO6 −23.5 −23 −22.5 −22 −21.5 −21 −20.5 −20 −19.5 −19 −18.5 −18 −6 −5.5 −5 −4.5 −4 X (m) Y ( m ) Figura 6.35: Posiciones del robot en los puntos de parada realizados (puntos negros), trayectoria estimada con EKF (puntos azules) y posiciones estimadas en los puntos de parada con EKF (puntos rojos). observar en la Figura 6.36. Al tener en cuenta el ángulo de balanceo se pueden detectar los desplazamientos transversales del robot y con el método presentado en la sección 6.2.2 se ha estimado este desplazamiento. En cuanto a la posición estimada con la odometŕıa y el DGPS, ambas son idénticas a las presentadas en la sección 6.4.2.1. En la tabla 6.8 se detallan las posiciones estimadas con dead-reckoning teniendo en cuenta encoders y brújula electromagnética (dr1); y por otro lado, teniendo en cuenta encoders, brújula electromagnética e inclinómetro (dr2). También se pueden observar los errores de cada posición estimada respecto a la posición del robot en cada punto de parada. Respecto al error relativo a la distancia introducido con el EKF, se aprecia en la Figura 6.37 que es mayor que el obtenido en la sección anterior. En principio, al mejorar la estimación de dead-reckoning, se espera que también mejore la estimación del EKF. Esto no ocurre en el experimento EXP-2 en concreto, pero el error, generalmente, śı que está dentro del rango requerido. Analizando nuevamente el error de los cálculos realizados con el EKF para obtener una estimación de la posición del robot, se vuelve a demostrar la mejor precisión en los datos calculados con el EKF que con los obtenidos con dead- reckoning y DGPS (véase la Figura 6.38). Como hemos dicho anteriormente, la diferencia entre las posiciones del robot en cada punto de parada tomadas con el 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 145 Tabla 6.7: Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con EKF (Xekf y Yekf ) y errores relativos a la distancia ∆x, ∆y y ∆. P X(m) Y(m) Xekf (m) Yekf (m) ∆x(m) ∆y(m) ∆(m) 1 -18.2976 -5.6453 -18.3088 -5.6469 0.0112 0.0017 0.0113 2 -18.6303 -5.6480 -18.6109 -5.6507 -0.0194 0.0027 0.0196 3 -19.0139 -5.6650 -18.9934 -5.6685 -0.0205 0.0035 0.0208 4 -19.4128 -5.6396 -19.3927 -5.6665 -0.0201 0.0269 0.0336 5 -19.7788 -5.5852 -19.7328 -5.6331 -0.0460 0.0479 0.0664 6 -20.1872 -5.5493 -20.1288 -5.5772 -0.0585 0.0279 0.0648 7 -20.5490 -5.4988 -20.4998 -5.5156 -0.0492 0.0168 0.0520 8 -20.9341 -5.4467 -20.8938 -5.4582 -0.0403 0.0115 0.0419 9 -21.3059 -5.3567 -21.2556 -5.3646 -0.0503 0.0080 0.05096 10 -21.6528 -5.2510 -21.6329 -5.2438 -0.0198 -0.0071 0.0211 11 -22.0263 -5.0923 -22.0107 -5.1242 -0.0156 0.0319 0.0355 12 -22.3936 -4.9695 -22.3844 -4.9669 -0.0093 -0.0026 0.0097 13 -22.7316 -4.8027 -22.7291 -4.8187 -0.0025 0.0160 0.0162 14 -23.0827 -4.6214 -23.0479 -4.6614 -0.0348 0.0401 0.0530 sistema externo y las estimadas con el EKF son mayores en el experimento EXP-2, cuando se tiene en cuenta el ángulo de balanceo para estimar los desplazamientos transversales del robot debido a las holguras. A pesar de esto, ha quedado demostrado la mejor estimación de la trayectoria descrita por el robot al utilizar este ángulo. Además, en el error de los cálculos realizados para estimar la posición del EKF (matriz P ), se mejora la componente del error de la coordenada X2 (equivale a la coordenada Y ). Este aumento de la precisión de la posición estimada se comprueba al comparar la Figura 6.34 y la Figura 6.38. Tras presentar los resultados se puede concluir que la precisión que se alcanza con el algoritmo de localización implementado es aceptable ya que está dentro del rango ±0.02 m exigido. Esto tiene una gran importancia para desarrollar el control del seguimento de trayectorias que se presenta en el caṕıtulo 7. 146 Algoritmo de localización para el Robot SILO6 −23.5 −23 −22.5 −22 −21.5 −21 −20.5 −20 −19.5 −19 −18.5 −18 −6.5 −6 −5.5 −5 −4.5 −4 X (m) Y ( m ) Figura 6.36: Puntos de parada del robot (puntos negros) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning -encoders y brújula electromagnética- (azul claro), dead- reckoning -encoders, brújula electromagnética e inclinómetro- (azul oscuro), DGPS (verde) y EKF (rojo). 0 2 4 6 8 10 12 14 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a Figura 6.37: Errores relativos a la distancia recorrida introducidos por EKF: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). 6.4 Fusión Sensorial mediante un filtro de Kalman para localización de robots caminantes en exteriores 147 Tabla 6.8: Paradas del robot (P ), posiciones estimadas con dr1 (Xdr1 y Ydr1), posiciones estimadas con dr2 (Xdr2 y Ydr2) y diferencia de errores relativos absolutos obtenidos con ∆dr1 y ∆dr2. P Xdr1(m) Ydr1(m) Xdr2(m) Ydr2(m) ∆dr1(m) ∆dr2(m) 1 -18.3121 -5.6474 -18.3121 -5.6474 0.0147 0.0147 2 -18.6315 -5.6834 -18.6319 -5.6796 0.0354 0.0316 3 -18.9959 -5.7244 -18.9984 -5.7038 0.0621 0.0419 4 -19.3658 -5.7518 -19.3690 -5.7318 0.1216 0.1021 5 -19.6782 -5.7619 -19.6819 -5.7215 0.2033 0.1673 6 -20.0483 -5.7487 -20.0520 -5.7116 0.2431 0.2113 7 -20.3654 -5.7216 -20.3694 -5.6884 0.2887 0.2612 8 -20.7185 -5.6926 -20.7221 -5.6549 0.3270 0.2971 9 -21.0467 -5.6445 -21.0481 -5.5911 0.3874 0.3485 10 -21.3995 -5.5762 -21.3988 -5.5112 0.4122 0.3637 11 -21.7589 -5.4974 -21.7529 -5.4072 0.4854 0.4170 12 -22.1286 -5.3918 -22.1179 -5.2853 0.4986 0.4192 13 -22.4496 -5.2870 -22.4365 -5.1714 0.5605 0.4723 14 -22.7705 -5.1695 -22.7579 -5.0541 0.6308 0.5410 148 Algoritmo de localización para el Robot SILO6 0 20 40 60 80 100 120 140 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 Medidas E rr or ( m ) Figura 6.38: Errores en la medida calculada con EKF: para la coordenada X1 (azul), para la coordenada X2 (rojo) y para la coordenada X3 (verde). Caṕıtulo 7 Seguimiento de trayectorias con máquinas caminantes 7.1. Introducción Existen diferentes aplicaciones de las máquinas caminantes en las cuales es necesario o conveniente que el veh́ıculo se desplace por trayectorias predeterminadas con el fin de optimizar su eficacia. Por ejemplo, una máquina caminante podŕıa resultar útil para trabajar en cultivos alineados en hileras (es decir, estructurados) mientras avanza por un terreno irregular (no estructurado), blando y deslizante. El seguimiento de trayectorias puede también resultar interesante en aplicaciones en las que sea necesario explorar por completo una determinada extensión de terreno, como ocurre en aplicaciones de desminado humanitario. En este caso, una posible estrategia consiste en planificar una trayectoria que garantice la exploración de toda la superficie minimizando el recorrido del veh́ıculo. Una de las principales ventajas de las máquinas caminantes frente a los veh́ıculos convencionales es su capacidad para desplazarse de manera omnidireccional (véase el caṕıtulo 1). Esta mayor libertad a la hora de decidir los movimientos de la máquina ofrece mayores posibilidades a la hora de seguir trayectorias. En los veh́ıculos no holonómicos1, como los basados en ruedas u 1Un veh́ıculo no holonómico es aquel en el cual una vuelta a la configuración interna original no garantiza una vuelta a la posición y orientación iniciales del sistema. Es decir, la posición y orientación del veh́ıculo son dependientes del camino seguido, y el número de coordenadas generalizadas necesarias para representar completamente el sistema es mayor que el número de grados de libertad controlados. 150 Seguimiento de trayectorias con máquinas caminantes orugas, la orientación del robot y su dirección de movimiento están estrechamente relacionadas. Por lo tanto, en general, un error en la posición del veh́ıculo debe corregirse cambiando su orientación momentáneamente hasta volver a la posición deseada (véase la Figura 7.1(a)). En una máquina caminante, por el contrario, es posible corregir la posición de manera que se sitúe sobre la trayectoria deseada sin necesidad de modificar su orientación. Esto se puede lograr empleando los modos de deriva, en los que el robot camina siguiendo una trayectoria que forma un cierto ángulo con su eje longitudinal (véase la Figura 7.1-b y la sección 2.3). Una consecuencia del desacoplo entre la orientación del veh́ıculo y su dirección de avance es que simplifica notablemente el guiado del veh́ıculo. Aśı, un veh́ıculo omnidireccional permite evitar las maniobras complejas como los sucesivos movimientos hacia delante y hacia atrás que es necesario realizar con los veh́ıculos con ruedas directrices para producir un desplazamiento lateral o para cambiar de orientación manteniendo la posición del centro de gravedad (CDG). Esta simplificación resulta útil tanto en el caso del seguimiento de trayectorias mediante guiado por un operario, ya que hace el manejo más intuitivo, como en el caso de control de trayectoria por un sistema automático de control, ya que simplifica su programación y funcionamiento y, además, se evita la planificación de maniobras complejas. Alternativamente, en un veh́ıculo omnidireccional es posible controlar la orientación del cuerpo sin que esto signifique necesariamente un cambio en la trayectoria seguida por el robot. Esta propiedad puede ser útil en el caso de que sea necesario orientar un sensor o una herramienta a bordo del robot mientras éste sigue una determinada trayectoria. De los tres niveles de control descritos en el caṕıtulo 1 para un robot caminante, en el caṕıtulo 3 se describió el nivel inferior (el modo de caminar), que permite considerar al robot caminante como una plataforma móvil que puede dirigirse y que presenta propiedades interesantes como la omnidireccionalidad. Este nivel inferior permite abstraer a los niveles superiores de la complejidad de la planificación de los movimientos de las patas. En este caṕıtulo se tratan los dos niveles superiores de planificación y control. Por un lado, se describen los métodos empleados para lograr que un veh́ıculo siga las trayectorias básicas especificadas, teniendo en cuenta y aprovechando las particularidades de las máquinas caminantes. Este nivel de control debe hacer uso de un sistema de localización como el descrito en el caṕıtulo 6. Por otro lado, se describe brevemente un posible algoritmo de cobertura completa del terreno propuesto por Garcia y Gonzalez de Santos (2004) como ejemplo de planificación de trayectorias de alto nivel para poner en práctica las técnicas desarrolladas: definición de 7.2 Definición de la trayectoria deseada 151 Trayectoria deseada Trayectoria deseada Trayectoria del robot Trayectoria del robot ( a ) ( b ) Figura 7.1: Seguimiento de trayectorias con un robot: a) basado en ruedas y b) basado en patas. trayectorias a seguir, estrategias de cambio de trayectorias, selección de modos de caminar, control de ángulo de deriva, control de orientación, etc. Esta estrategia de cobertura completa también se empleará como ejemplo para realizar las simulaciones y experimentos que prueban la validez del sistema completo. En la sección 7.2 se describe la estrategia de cobertura completa empleada, y en la sección 7.3 se describen las técnicas utilizadas para lograr que el robot siga las trayectorias especificadas. Finalmente, las secciones 7.4 y 7.5 describen las simulaciones y experimentos empleados para probar la validez del método. 7.2. Definición de la trayectoria deseada La trayectoria a seguir por un robot móvil puede definirse siguiendo distintos criterios. En general, estos criterios dependerán de las caracteŕısticas de la aplicación. En el caso de que el objetivo sea trasladar el robot a un punto dado, la 152 Seguimiento de trayectorias con máquinas caminantes planificación de trayectorias tratará únicamente de determinar un camino óptimo desde el punto de vista del recorrido, evitando las zonas prohibidas del terreno (Gonzalez de Santos y Jimenez, 1995). En el caso de aplicaciones como el desminado humanitario el objetivo que se persigue mediante el seguimiento de trayectorias es la exploración eficiente y completa de una superficie. Es decir, se pretende explorar con sensores una superficie minimizando la distancia recorrida por la máquina caminante con el fin de agilizar un proceso que es siempre lento. Además, es imprescindible garantizar que todo el terreno a explorar sea efectivamente inspeccionado. El método empleado en esta tesis doctoral para determinar la trayectoria a seguir se describe brevemente en esta sección, a pesar de no constituir una aportación original, por las implicaciones que tiene en las técnicas de seguimiento de trayectorias desarrolladas. Para planificar la trayectoria se ha optado por el algoritmo de exploración completa propuesto por (Garcia y Gonzalez de Santos, 2004), que es a su vez una variante del método propuesto por (Acar y Choset, 2001). En este método las trayectorias se planifican en dos dimensiones, sobre el plano horizontal. La tercera coordenada, es decir, la altura del veh́ıculo no se especifica y se determina en tiempo real a partir de datos sensoriales que informen acerca del perfil del terreno y con el fin de adaptarse a éste. Cuando el terreno es practicable, el robot sigue trayectorias rectiĺıneas paralelas entre śı que se recorren en sentidos alternos. Estas trayectorias, en las que se produce la exploración del terreno, están conectadas entre śı por trayectorias también rectiĺıneas y perpendiculares (véase la Figura 7.2). Las trayectorias de exploración están separadas una distancia igual a la anchura de la franja de terreno que el robot es capaz de explorar con sus sensores, con lo que se consigue una cobertura completa y eficiente. Es decir, el robot explora franjas de terreno paralelas y contiguas alternando el sentido de avance. Este esquema de funcionamiento se mantiene mientras la irregularidad del terreno o la presencia de obstáculos no impidan el avance. En el caso de que los sensores detecten un obstáculo o una irregularidad excesiva el robot pasará a explorar la franja de terreno contigua, cambiando, por tanto, su dirección de avance (véase la Figura 7.2). Aśı, la detección de un obstáculo supone una división del terreno en dos celdas, de las cuales inicialmente se explora sólo una (véase la Figura 7.2). Por esta razón, la posición de cada obstáculo se registra de manera que posteriormente el robot pueda rastrear las celdas de terreno no exploradas en el primer recorrido. El aspecto más relevante del algoritmo de exploración seleccionado para el 7.3 Control de seguimiento de trayectoria 153 Ancho de exploración Trayectoria predefinida Área explorada Obstáculo Trayectoria predefinida Área no explorada Trayectoria determinada en tiempo de ejecución Figura 7.2: Trayectoria de exploración completa del terreno. seguimiento de trayectorias es que el robot debe seguir por defecto trayectorias predefinidas, concretamente segmentos rectiĺıneos concatenados. Por esta razón, se definirá la trayectoria mediante una sucesión de puntos que constituyen el comienzo de un segmento y el fin del anterior. Aśı, el segmento i de la trayectoria, Si, está definido por las coordenadas horizontales (x, y) del punto de comienzo P i y del punto final P i+1 medidos en un sistema de referencia del terreno, (XT , YT ) (véase la Figura 7.3). De estos dos puntos se deduce la orientación del segmento ψi con respecto a XT : ψi =    arctan ( P i+1 y −P i y P i+1 x −P i x ) si P i x 6= P i+1 x P i+1 y −P i y |P i+1 y −P i y| π 2 e.o.c (7.1) 7.3. Control de seguimiento de trayectoria Para seguir una trayectoria según la definición dada en la sección 7.2 es necesario controlar tres grados de libertad del robot: el ángulo azimutal del robot, φ, que es el ángulo formado por el eje XR del sistema del robot (coincidente 154 Seguimiento de trayectorias con máquinas caminantes Segmento Si Dirección de avance fD a P i P i+1 XT Sistema de referencia del terreno dPR i(CDG,S) vSi R y vector uSi CDG ef f yi YT XR Figura 7.3: Estrategia de control de trayectoria. con el eje longitudinal de su cuerpo) y el eje XT del sistema de referencia del terreno, y por otro lado las coordenadas horizontales de posición de su centro de gravedad, CDGX y CDGY (véase la Figura 7.3). Dado que la coordenada vertical de posición del cuerpo no se especifica, ésta se determinará en tiempo real en función del perfil del terreno a través del controlador de velocidad vertical descrito en la sección 3.8. Lo mismo sucede con los ángulos de cabeceo y balanceo del cuerpo, que se ajustarán para mantener el cuerpo horizontal como se explica en la sección 3.8. El seguimiento de la trayectoria se logra mediante un módulo de control llamado controlador de trayectoria que modifica ciertos parámetros del modo de caminar, o es incluso capaz de seleccionar entre varios modos de caminar en función de la trayectoria a seguir. Los parámetros del modo de caminar seleccionados para controlar el movimiento del robot son el ángulo de deriva, α, la velocidad angular de guiñada, ωCG, y la velocidad de avance del cuerpo, VCH . Los modos de caminar que se pueden seleccionar son el modo de avance y modo rotatorio (giro de radio nulo) descrito en el caṕıtulo 3. Este controlador de trayectoria se compone de cinco controladores más simples que se encargan de: control azimutal, control de posición horizontal, control de velocidad de avance, control cambio de trayectoria y selección de modo de caminar. 7.3 Control de seguimiento de trayectoria 155 7.3.1. Control azimutal del robot El ángulo azimutl deseado, φD, es la orientación del segmento i de la trayectoria que se está siguiendo en ese instante. Es decir: φD = ψi (7.2) Para mantener el cuerpo del robot con esta orientación se regula la velocidad angular de guiñada ωCG empleando una ley de control proporcional: ωCG = Kpφεφ (7.3) donde Kpφ es la ganancia del controlador y εφ = φ− φD (7.4) se denomina error de azimut. Esta simple ley de control se muestra lo suficientemente efectiva tanto para corregir los pequeños errores que se producen durante el seguimiento de un segmento como para originar los grandes cambios de orientación en los puntos que conectan distintos segmentos, dada la baja velocidad a la que se produce el giro del robot. 7.3.2. Control de posición horizontal del robot La posición del robot se controla de manera que el CDG se sitúe sobre la trayectoria deseada. De esta forma se corrigen los errores en la alineación del robot y los desplazamientos laterales producidos por flexiones y holguras de las patas o deslizamientos de los pies. Para lograrlo, se vaŕıa el ángulo de deriva según una ley de control proporcional con saturación: α= { σKpαδPR ( CDG, Si ) si |α|<αmax σαmax e.o.c (7.5) donde δR(CDG, Si) es la distancia del CDG a la trayectoria actual Si y donde Kpα es la ganancia del controlador. El valor de σ se define de manera que el signo de la deriva sea el apropiado para acercarse a la trayectoria: σ= { 1 si q > 0 −1 si q < 0 −→uSi= ( Pi+1 X − P i X,Pi+1 Y − P i Y )T −→ vR Si = ( CDGX − Pi X, CDGY − Pi Y )T (7.6) 156 Seguimiento de trayectorias con máquinas caminantes donde −→uSi × −→ vR Si = q −→ k , −→uSi es el vector que define el segmento Si, y −→vSi es el vector que va del punto inicial del segmento Si al CDG del robot. Este simple controlador proporcional se muestra también suficientemente efectivo para controlar el robot, dadas las caracteŕısticas del sistema y la baja velocidad de avance. Este controlador asume que el error de azimut del robot es lo suficientemente reducido como para no degradar el control de posición. El valor máximo del ángulo de deriva, αmax, se fija de manera que en casos extremos se produzca un desplazamiento lateral pero no se invierta el sentido de la marcha (αmax = π/2, es decir, −π/2 < α < π/2 si el robot se desplaza normalmente hacia delante). 7.3.3. Control de velocidad de avance del robot Durante el seguimiento de un segmento, la velocidad de avance se mantiene constante en un valor nominal apropiado para la exploración del terreno, VCHN . Dado que esta velocidad no es cŕıtica, (siempre que no supere la velocidad máxima de exploración), la velocidad de avance se controla en lazo abierto siguiendo el siguiente esquema: VCH= { VCHN si |εφ| < εφ máx 0 e.o.c. (7.7) Es decir, si el error de azimut supera un cierto valor, εφmax, debido por ejemplo a un deslizamiento de los pies, se detiene el avance permitiendo aśı al controlador azimutal recuperar una orientación adecuada. En otro caso, el robot avanza a la velocidad nominal de exploración. 7.3.4. Control de cambio de trayectoria y selección de modo de caminar En el seguimiento de una trayectoria, el cambio de un segmento, Si, al siguiente segmento, Si+1, se produce cuando el robot sobrepasa la recta perpendicular a Si que pasa por su punto final, P i+1. De esta manera se garantiza que se producirá el cambio de trayectoria a pesar de que el robot pase lejos del punto final de la trayectoria actual debido a errores de control o perturbaciones, y sin disminuir por ello la precisión en el seguimiento. Matemáticamente, el cambio de la trayectoria Si a la trayectoria Si+1 se produce cuando se da la siguiente condición: 7.3 Control de seguimiento de trayectoria 157 S i-1 S i S i+1 Recta de transición de Si+1Si a Recta de transición de S i-1 a S i w Si R v Si R u Si u Si w Si R = 0 P i+1 P i-1 P i Figura 7.4: Cambio de trayectoria. −→uSi · −→ wR Si ≥ 0 (7.8) donde −→uSi= ( Pi+1 X − P i X, Pi+1 Y − P i Y ) −→ wR Si = ( Pi+1 X −RX , Pi+1 Y −RY ) (7.9) Es decir, el cambio se produce cuando el vector que une P i+1 y el CDG del robot ( −→ wR Si ) y el vector que define el segmento actual −→uSi forman un ángulo entre π/2 y −π/2 (véase la Figura 7.4). Al comenzar el seguimiento de cada segmento se selecciona el modo de giro de radio nulo hasta que se anule el error de azimut, es decir, hasta que el robot quede alineado con la nueva trayectoria. Una vez alineado, se selecciona el modo de caminar de deriva para seguir el nuevo segmento. 158 Seguimiento de trayectorias con máquinas caminantes 7.4. Valoración del algoritmo mediante simulación Para comprobar la efectividad del seguimiento de trayectorias con máquinas caminantes se han realizado varias simulaciones, particularizadas para el caso del robot SILO6. En todas las simulaciones los valores de las ganancias de los controladores empleados por el controlador de trayectorias son: Kpα = 0.005 y Kpφ = 0.2. Se considera también que εφmáx = 5o. En la primera simulación el robot debe seguir una trayectoria rectiĺınea con orientación 0o. La posición y la orientación iniciales del robot no coinciden con las de la trayectoria a seguir. Concretamente, el centro del robot está desplazado en la coordenada y 200 mm en el sentido negativo y con una orientación de −20o. Los resultados se presentan en la Figura 7.5. Inicialmente el robot disminuye el error de orientación, utilizando el modo rotatorio, hasta que en t = 20 s aproximadamente el error se reduce a 5o (véase la Figura 7.5(b)). En ese momento comienza a desplazarse con el modo de deriva, reduciendo el error de posición y de orientación simultáneamente. Cuando el robot se ha desplazado 1500 mm aproximadamente se alcanza la posición y la 0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000 5500 −1000 −500 0 500 1000 (a) X (mm) Y ( m m ) 0 50 100 150 200 250 300 −30 −20 −10 0 10 20 30 (b) Tiempo (s) O rie nt ac ió n (º ) Figura 7.5: Simulación 1: (a) Posición del centro geométrico del robot en el plano XY y (b) orientación del robot en función del tiempo. 7.4 Valoración del algoritmo mediante simulación 159 Figura 7.6: Simulación 2: (a) Posición del centro geométrico del robot y (b) orientación del robot. orientación deseados (vease la Figura 7.5(a)). En la segunda simulación el robot debe seguir una trayectoria rectiĺınea con orientación 0o y en determinados instantes (t = 30, 60, 90, 120 s) se introducen cambios bruscos de orientación que en casos reales pueden ser debidos a las flexiones u holguras en las articulaciones de las patas o deslizamientos de los pies. Los cambios introducidos se pueden apreciar en la Figura 7.6(b) cuyas magnitudes son: 5o, 10o, −5o y −10o, respectivamente. Ante cada salto en la orientación el robot tiende al valor de la consigna. Cada cambio en la orientación se traduce en una desviación de la posición del centro geométrico del robot de la trayectoria a seguir (véase la Figura 7.6(a)). Se observa cómo estas variaciones en la posición dependen del signo y magnitud de los cambios de orientación y también cómo el seguimiento de trayectorias funciona correctamente ya que va recuperando a la vez la posición y orientación que el robot debe seguir. En la tercera simulación el robot debe seguir una trayectoria rectiĺınea con orientación 0o y en determinados instantes se introducen cambios bruscos en la coordenada y de la posición del robot. Estos cambios, al igual que en la segunda simulación, pueden ser debidos a las flexiones u holguras en las articulaciones de las patas o deslizamientos de los pies. En esta simulación se describen los desplazamientos transversales que se presentaron en la sección 6.2.2. Los cambios 160 Seguimiento de trayectorias con máquinas caminantes se producen cada 40 s y sus valores, en orden de aparición, son: 5 mm, −10 mm, 10 mm y −5 mm. En la simulación se observa cómo el robot recupera la posición mediante el modo de deriva manteniendo su orientación (véase la Figura 7.7(a)). Como los cambios sólo se producen en la posición, la orientación del robot se mantiene constante y en el valor inicial, 0o, (véase la Figura 7.7(b)). Por último, en el cuarto ejemplo de simulación el robot sigue una trayectoria similar a la presentada en la Figura 7.2. Se realizan dos pruebas en las que se evalúa el comportamiento del modo de caminar en presencia y en ausencia de zonas prohibidas. En la Figura 7.8 se observan las posiciones del centro geométrico del robot y la orientación cuando no existen zonas prohibidas sobre el terreno. El robot sigue la trayectoria predefinida sin errores importantes (véase la Figura 7.8(a)) y en la variación de la orientación se aprecia cuándo se realizan los giros y cuándo se mantiene constante (véase la Figura 7.8(b)). Al realizar la simulación teniendo en cuenta las zonas prohibidas, se comprueba que la trayectoria descrita por el robot y su orientación no sufren variaciones (véase la Figura 7.9). La existencia de zonas prohibidas implica una variación en los puntos de apoyo de los pies, que genera el modo de caminar (véase la sección 3.11), pero esto no influye en la posición y orientación del robot 0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000 −500 0 500 (a) X (mm) Y ( m m ) 0 50 100 150 200 250 300 350 400 450 −1 −0.5 0 0.5 1 (b) Tiempo (s) O rie nt ac ió n (º ) Figura 7.7: Simulación 3: (a) Posición del centro geométrico del robot y (b) orientación del robot. 7.5 Valoración del algoritmo experimentalmente 161 −2000 0 2000 4000 −1000 0 1000 2000 3000 (a) X (mm) Y ( m m ) 0 200 400 600 800 1000 1200 1400 1600 0 50 100 150 200 (b) Tiempo (s) O rie nt ac ió n (º ) Figura 7.8: Simulación 4: (a) Posición del centro geométrico del robot y (b) orientación del robot sin tener en cuenta las zonas prohibidas sobre el terreno. como se demuestra al comparar la Figura 7.8 y la Figura 7.9. El efecto de estas zonas prohibidas se puede comprobar en la Figura 7.10 y Figura 7.11. En ambas se presentan las zonas prohibidas, pero en la Figura 7.10 no se tienen en cuenta en el modo de caminar, por lo tanto los pies se pueden apoyar sobre ellas. En la Figura 7.11, por el contrario, śı se tienen en cuenta las zonas prohibidas. Se puede apreciar, como en la tercera simulación de la sección 3.11, que los puntos de apoyo de los pies no pisan sobre una zona prohibida (véase la Figura 7.11). Un aumento de la misma zona en la Figura 7.10 y la Figura 7.11 demuestra que los pies del robot, al conocer la existencia de las zonas prohibidas, no pisan sobre ellas y, además, la posición y orientación del robot se mantienen con los valores deseados para el seguimiento de trayectorias (véase la Figura 7.9). 7.5. Valoración del algoritmo experimentalmente En esta sección se presentan los experimentos llevados a cabo con el robot SILO6 para comprobar el funcionamiento del algoritmo de control de seguimiento de trayectorias implementado. Los experimentos realizados son: (a) seguimiento 162 Seguimiento de trayectorias con máquinas caminantes −2000 0 2000 4000 −1000 0 1000 2000 3000 (a) X (mm) Y ( m m ) 0 200 400 600 800 1000 1200 1400 1600 0 50 100 150 200 (b) Tiempo (s) O rie nt ac ió n (º ) Figura 7.9: Simulación 4: (a) Posición del centro geométrico del robot y (b) orientación del robot teniendo en cuenta las zonas prohibidas sobre el terreno. de trayectoria rectiĺınea (EXP-3) y (b) seguimiento de trayectoria de exploración completa (similar a la trayectoria presentada en la Figura 7.2) sobre terreno irregular (EXP-4). En ambos experimentos se presentarán estimaciones de la posición del robot obtenidos con diferentes métodos, como se hizo en la sección 6.4.2. 7.5.1. EXP-3: Seguimiento de una trayectoria rectiĺınea El experimento EXP-3 consiste en desplazar el robot a lo largo de una trayectoria recta cuyo origen coincide con el centro geométrico del cuerpo del robot y su orientación con del robot en el instante inicial del desplazamiento. En la Figura 7.12 se presenta en rojo la trayectoria a seguir y en negro la trayectoria descrita por el robot. Los métodos para estimar la posición y orientación del robot descritos en esta sección son: odometŕıa, dead-reckoning, DGPS y EKF. El EKF se utiliza teniendo en cuenta por un lado: los encoders, una brújula electromagnética y un DGPS (como en la sección 6.4.2.1); y, por otro lado, los encoders, un inclinómetro, una brújula electromagnética y un DGPS (como en la sección 6.4.2.2). 7.5 Valoración del algoritmo experimentalmente 163 -1000 -500 0 500 1000 1500 2000 2500 3000 3500 -500 0 500 1000 1500 2000 2500 3000 X (mm) Y (m m ) (a) Figura 7.10: Modo de caminar sobre terreno sin tener en cuenta zonas prohibidas. Puntos de apoyo de patas delanteras (ćırculo azul), puntos de apoyo de patas centrales (ćırculo rojo) y puntos de apoyo de patas traseras (ćırculo negro). Debido a las imperfecciones del mecanismo, al igual que ocurŕıa en el EXP- 2 (véase la sección 6.4.2), el robot tiende a apartarse de la trayectoria (véase la Figura 6.30). La diferencia entre ambos experimentos es que en este caso se pueden corregir las desviaciones de la trayectoria deseada al implementear el algoritmo de control de seguimiento de trayectorias presentado en este caṕıtulo. Tanto en el EXP-2 como en el EXP-3 la posición y orientación iniciales del robot son las mismas. La mejoŕıa introducida con el seguimiento de trayectorias es evidente (véanse la Figura 6.30 y la Figura 7.12). En la Figura 7.13 se pueden observar las distintas estimaciones de la posición del robot en EXP-3 empleando los diferentes métodos de estimación propuestos junto a la trayectoria deseada. Claramente, la mejor estimación se obtiene tanto con el EKF como con el DGPS. En el resto de estimaciones el error aumenta cuanto más se desplaza 164 Seguimiento de trayectorias con máquinas caminantes -1000 -500 0 500 1000 1500 2000 2500 3000 3500 -500 0 500 1000 1500 2000 2500 3000 X (mm) Y (m m ) (b) Figura 7.11: Modo de caminar sobre terreno teniendo en cuenta zonas prohibidas. Puntos de apoyo de patas delanteras (ćırculo azul), puntos de apoyo de patas centrales (ćırculo rojo) y puntos de apoyo de patas traseras (ćırculo negro). el robot. Hay que resaltar que la implementación del algoritmo de control de seguimiento de trayectorias mejora considerablemente las estimaciones de odometŕıa y dead-reckoning respecto al EXP-2. Esto se debe a que en el EXP- 3 se sigue una trayectoria recta y está activado el control de seguimiento de la trayectoria, por lo tanto, no se producen variaciones apreciables en la orientación (3.75o en el EXP-3) lo cual influye en dead-reckoning y, sobre todo, en odometŕıa. Esto puede afirmarse observando los errores relativos con la distancia recorrida con dead-reckoning -dr1 y dr2- (véase la sección 6.4.2.2.) y con odometŕıa (véanse la Figura 7.14, la Figura 7.15 y la Figura 7.16). Se aprecia cómo estos errores son parecidos, al contrario de lo que se ha obtenido en el EXP-2 y, por lo tanto, evidencia una mejoŕıa de la odometŕıa. En EXP-3 la mejoŕıa introducida al tener en cuenta el inclinómetro en dead- reckoning, dr2, se aprecia muy poco (véase la Figura 7.13). Esto es de esperar ya que al estimar un desplazamiento transversal, con el modo de deriva y de giro, el 7.5 Valoración del algoritmo experimentalmente 165 Figura 7.12: Trayectoria descrita y posición final del robot SILO6 para el experimento EXP-3. 166 Seguimiento de trayectorias con máquinas caminantes robot recupera la trayectoria a seguir. Respecto a la estimación con el EKF se observa una cierta mejoŕıa con respecto al EXP-2. Como se puede apreciar en la tabla 7.4 y la Figura 7.17. El rango de los valores de los errores relativos son muy parecidos, por lo tanto, dentro de la precisión requerida. Todas las posiciones estimadas y errores obtenidos con los diferentes métodos en los puntos de parada del robot se presentan en las tablas 7.1, 7.2, 7.3, 7.4, y 7.5. En la tabla 7.1 se presentan las posiciones estimadas por la odometŕıa, las posiciones en cada punto de parada del robot y los errores introducidos por la odometŕıa. En la tabla 7.2 se presentan los mismos datos, pero con las posiciones estimadas con dead-reckoning a partir de los encoders y la brújula electromagnética, dr1. En la tabla 7.3 también se muestran los datos de dead-reckoning pero a partir de los encoders, el inclinómetro y la brújula electromagnética, dr2. En la tabla 7.4 se presentan todo lo referido a la estimación con el EKF cuando la fase de estimación se realiza con dr2. En la tabla 7.5 se comparan las posiciones estimadas con dr1 y dr2 (véase la sección 6.4.2.2.) aśı como sus errores. Finalmente, se puede concluir que en el EXP-3 se obtienen resultados satisfactorios con el modo de caminar y el sistema de localización implementados en el robot SILO6. Ambos aspectos son complementarios: por un lado, el modo de caminar necesita un buen conocimiento de la estimación de la posición y orientación del robot; y por otro lado, para el correcto seguimiento de una trayectoria es imprescindible un buen funcionamiento del algoritmo de control de seguimiento de trayectoria. 7.5.2. EXP-4: Trayectoria de exploración completa El experimento EXP-4 consiste en desplazar el robot a lo largo de una trayectoria de exploración completa para barrer un campo minado. El origen de la trayectoria coincide con el centro geométrico del robot y su orientación con la del robot en el instante inicial del desplazamiento. En la Figura 7.18 se presenta el terreno donde tiene lugar el experimento EXP-4. Los segmentos que recorre el robot, indicados en la Figura 7.18, tienen las siguientes dimensiones: segmento1 = 2 m, segmento2 = 0.750 m, segmento3 = 2 m y segmento4 = 0.750 m. Todos juntos constituyen lo que denominamos un ciclo completo de exploración. A diferencia de los experimentos anteriores, en este caso no se medirá la 7.5 Valoración del algoritmo experimentalmente 167 Tabla 7.1: Paradas del robot (P ), posiciones del robot (X y Y ), posiciones estimadas con odometŕıa (Xo y Yo) y errores ∆x, ∆y y ∆. P X(m) Y(m) Xo(m) Yo(m) ∆x(m) ∆y(m) ∆(m) 1 -18.2983 -5.6789 -18.3037 -5.6779 0.0054 -0.0009 0.0055 2 -18.5894 -5.6776 -18.5479 -5.7420 -0.0415 0.0643 0.0766 3 -18.9565 -5.7976 -18.8945 -5.8377 -0.0620 0.0401 0.0738 4 -19.3404 -5.8215 -19.2388 -5.9130 -0.1016 0.0914 0.1367 5 -19.6814 -5.8850 -19.5744 -5.9980 -0.1070 0.1130 0.1556 6 -20.0331 -5.9582 -19.9152 -6.0741 -0.1179 0.1158 0.1653 7 -20.4044 -5.9921 -20.2525 -6.1594 -0.1519 0.1673 0.2260 8 -20.7643 -6.0861 -20.5790 -6.2451 -0.1853 0.1590 0.2442 9 -21.1240 -6.1384 -20.9125 -6.2900 -0.2115 0.1516 0.2602 10 -21.4881 -6.1669 -21.2509 -6.3531 -0.2372 0.1862 0.3016 11 -21.8472 -6.2225 -21.5800 -6.4381 -0.2673 0.2156 0.3434 12 -22.2142 -6.3132 -21.9306 -6.5190 -0.2836 0.2059 0.3505 13 -22.5589 -6.3652 -22.2422 -6.5827 -0.3167 0.2175 0.3842 14 -22.9372 -6.3942 -22.5688 -6.6558 -0.3684 0.2616 0.4518 15 -23.2921 -6.4853 -22.8926 -6.7272 -0.3994 0.2420 0.4670 16 -23.6678 -6.5299 -23.1999 -6.7893 -0.4678 0.2595 0.5350 17 -24.0357 -6.5940 -23.5467 -6.8685 -0.4890 0.2745 0.5608 18 -24.4000 -6.6791 -23.9043 -6.9457 -0.4958 0.2666 0.5629 19 -24.7767 -6.6924 -24.2018 -6.9978 -0.5749 0.3053 0.6510 posición del robot con un sistema externo y, por lo tanto, no se realizarán paradas intermedias para conocer la posición del robot con precisión. En el caṕıtulo 6 se han realizado los experimentos necesarios para conocer los errores en la estimación de la posición con cada método utilizado. Conocidos éstos, en el EXP-4 nos interesa estudiar la desviación que se produce en la trayectoria seguida por el robot respecto a la trayectoria deseada. Para ello, consideraremos el error como la diferencia entre la estimación de la trayectoria con cada método utilizado y la trayectoria a seguir (véase la Figura 7.19). La mejor estimación es la obtenida con el EKF. Con DGPS se obtienen muy buenos resultados pero con el problema siempre presente de la posible pérdida de señal de los satélites. En cuanto a dead-reckoning, dr2 vuelve a estimar mejor la posición que dr1. En este terreno cobra un papel más importante el Método del Balanceo (véase la sección 6.2.2.) y por lo tanto se aprecia perfectamente la 168 Seguimiento de trayectorias con máquinas caminantes Tabla 7.2: Paradas del robot (P ), posiciones del robot, posiciones estimadas con dead-reckoning (sin Método de Balanceo) y errores. P X(m) Y(m) Xdr(m) Ydr(m) ∆x(m) ∆y(m) ∆(m) 1 -18.2983 -5.6789 -18.3037 -5.6798 0.0054 -0.0009 0.0055 2 -18.5894 -5.6776 -18.5492 -5.7387 -0.0402 0.0611 0.0732 3 -18.9565 -5.7976 -18.8960 -5.8333 -0.0605 0.0357 0.0703 4 -19.3404 -5.8215 -19.2406 -5.9072 -0.0998 0.0857 0.1316 5 -19.6814 -5.8850 -19.5761 -5.9908 -0.1053 0.1058 0.1493 6 -20.0331 -5.9582 -19.9167 -6.0659 -0.1163 0.1077 0.1585 7 -20.4044 -5.9921 -20.2556 -6.1456 -0.1488 0.1535 0.2137 8 -20.7643 -6.0861 -20.5822 -6.2310 -0.1821 0.1449 0.2327 9 -21.1240 -6.1384 -20.9154 -6.2769 -0.2086 0.1385 0.2504 10 -21.4881 -6.1669 -21.2544 -6.3399 -0.2337 0.1730 0.2908 11 -21.8472 -6.2225 -21.5844 -6.4209 -0.2628 0.1984 0.3293 12 -22.2142 -6.3132 -21.9351 -6.5008 -0.2791 0.1876 0.3363 13 -22.5589 -6.3652 -22.2470 -6.5634 -0.3119 0.1982 0.3695 14 -22.9372 -6.3942 -22.5737 -6.6362 -0.3635 0.2420 0.4367 15 -23.2921 -6.4853 -22.8972 -6.7082 -0.3948 0.2230 0.4534 16 -23.6678 -6.5299 -23.2048 -6.7687 -0.4629 0.2388 0.5208 17 -24.0357 -6.5940 -23.5525 -6.8439 -0.4833 0.2500 0.5441 18 -24.4000 -6.6791 -23.9100 -6.9217 -0.4901 0.2426 0.5468 19 -24.7767 -6.6924 -24.2080 -6.9720 -0.5687 0.2795 0.6337 mejor estimación. El problema más importante que aparece al llevar a cabo esta trayectoria tiene lugar en los giros. Se observa que la posición del robot se aleja de la que debeŕıa tener en esos instantes (véase la Figura 7.19). Esto se debe a que en estas situaciones, durante el tiempo en que se ejecuta el modo de giro para que el robot alcance la siguiente trayectoria, no se estima la posición del robot. En el momento en que el robot comienza a desplazarse hacia delante, cuando la diferencia entre la orientación del nuevo trayecto y su orientación es menor de 5o, se vuelve a estimar la posición del robot y, lógicamente, el robot vuelve a alcanzar la trayectoria a seguir en pocos segundos (véase la Figura 7.19). Los errores son analizados por segmentos. En EXP-4 son necesarios cuatro segmentos para realizar un ciclo completo de la trayectoria de exploración. Cuando se producen los giros no se calcula el error porque no se estima la posición 7.5 Valoración del algoritmo experimentalmente 169 Tabla 7.3: Paradas del robot (P ), posiciones del robot, posiciones estimadas con dead-reckoning (con Método de Balanceo) y errores. P X(m) Y(m) Xdr(m) Ydr(m) ∆x(m) ∆y(m) ∆(m) 1 -18.2983 -5.6789 -18.3037 -5.6798 0.0054 -0.0009 0.0055 2 -18.5894 -5.6776 -18.5494 -5.7371 -0.0400 0.0595 0.0717 3 -18.9565 -5.7976 -18.8984 -5.8175 -0.0581 0.0199 0.0614 4 -19.3404 -5.8215 -19.2424 -5.8992 -0.0980 0.0777 0.1251 5 -19.6814 -5.8850 -19.5781 -5.9783 -0.1033 0.0934 0.1393 6 -20.0331 -5.9582 -19.9198 -6.0480 -0.1133 0.0897 0.1445 7 -20.4044 -5.9921 -20.2590 -6.1235 -0.1454 0.1314 0.1960 8 -20.7643 -6.0861 -20.5846 -6.2127 -0.1798 0.1266 0.2199 9 -21.1240 -6.1384 -20.9156 -6.2704 -0.2084 0.1320 0.2466 10 -21.4881 -6.1669 -21.2547 -6.3358 -0.2334 0.1689 0.2881 11 -21.8472 -6.2225 -21.5859 -6.4071 -0.2613 0.1846 0.3199 12 -22.2142 -6.3132 -21.9337 -6.5055 -0.2805 0.1923 0.3400 13 -22.5589 -6.3652 -22.2470 -6.5613 -0.3118 0.1960 0.3684 14 -22.9372 -6.3942 -22.5739 -6.6352 -0.3633 0.2410 0.4360 15 -23.2921 -6.4853 -22.8967 -6.7120 -0.3954 0.2268 0.4558 16 -23.6678 -6.5299 -23.2037 -6.7773 -0.4640 0.2474 0.5259 17 -24.0357 -6.5940 -23.5513 -6.8535 -0.4844 0.2596 0.5496 18 -24.4000 -6.6791 -23.9094 -6.9265 -0.4906 0.2474 0.5495 19 -24.7767 -6.6924 -24.2093 -6.9645 -0.5674 0.2720 0.6293 del robot. El estudio del error se centra en el error absoluto transversal introducido en cada estimación ya que, al conocer la trayectoria a seguir, se puede calcular la desviación de la posición estimada en cada instante. Al no existir puntos de parada del robot no se puede comparar la posición estimada con otra más precisa como en EXP-1, EXP-2 y EXP-3. Respecto al analisis del error longitudinal, es d́ıficil estimar los puntos donde debeŕıa estar el robot en cada instante de medición porque la velocidad del robot, y más en terreno irregular, es imprecisa y además en ocasiones los datos de los sensores no se toman cada segundo. Todo esto hace d́ıficil el estudio del error longitudinal. También hay que señalar que el error que se presenta es el que más interés tiene cuando se trata de que el robot siga una determinada trayectoria. El error transversal absoluto obtenido con dead-reckoning es el mayor, como en los experimentos presentados anteriormente. En EXP-4 se observa también el 170 Seguimiento de trayectorias con máquinas caminantes Tabla 7.4: Paradas del robot (P ), posiciones del robot, posiciones estimadas con EKF (con Método de Balanceo) y errores. P X(m) Y(m) Xekf (m) Yekf (m) ∆x(m) ∆y(m) ∆(m) 1 -18.2983 -5.6789 -18.3091 -5.6761 0.0109 -0.0028 0.0112 2 -18.5894 -5.6776 -18.5700 -5.6989 -0.0194 0.0213 0.0288 3 -18.9565 -5.7976 -18.9426 -5.7827 -0.0139 -0.0149 0.0203 4 -19.3404 -5.8215 -19.3052 -5.8446 -0.0351 0.0230 0.0419 5 -19.6814 -5.8850 -19.6524 -5.9045 -0.0290 0.0196 0.0350 6 -20.0331 -5.9582 -20.0048 -5.9617 -0.0283 0.0034 0.0285 7 -20.4044 -5.9921 -20.3678 -5.9971 -0.0366 0.0050 0.0370 8 -20.7643 -6.0861 -20.7161 -6.0890 -0.0483 0.0029 0.0484 9 -21.1240 -6.1384 -21.0822 -6.1462 -0.0418 0.0078 0.0426 10 -21.4881 -6.1669 -21.4535 -6.1869 -0.0346 0.0200 0.0400 11 -21.8472 -6.2225 -21.8093 -6.2298 -0.0379 0.0073 0.0386 12 -22.2142 -6.3132 -22.1745 -6.3137 -0.0397 0.0005 0.0397 13 -22.5589 -6.3652 -22.5285 -6.3530 -0.0304 -0.0122 0.0328 14 -22.9372 -6.3942 -22.8925 -6.4046 -0.0447 0.0105 0.0459 15 -23.2921 -6.4853 -23.2564 -6.4825 -0.0356 -0.0028 0.0358 16 -23.6678 -6.5299 -23.6201 -6.5285 -0.0476 -0.0015 0.0477 17 -24.0357 -6.5940 -24.0130 -6.5929 -0.0227 -0.0010 0.0228 18 -24.4000 -6.6791 -24.3894 -6.6739 -0.0107 -0.0051 0.0119 19 -24.7767 -6.6924 -24.7165 -6.6909 -0.0603 -0.0016 0.0603 menor error con dr2 respecto a dr1 (véanse la Figura 7.20 y la Figura 7.21). Salvo en el primer trayecto, en el que los errores son del mismo orden, se observa cómo el error con dr1 siempre es mayor que con dr2. Hay que indicar la suavidad de la evolución del error absoluto transversal en dr1 (véase la Figura 7.20). En dr2 la estimación de los desplazamientos transversales del robot a partir del ángulo de balanceo provoca que la evolución del error absoluto transversal sea menos suave (véase la Figura 7.21). Con el DGPS el error absoluto transversal que se obtiene es mucho menor que los obtenidos con dr1 y dr2 (véase la Figura 7.22). El error suele estar por debajo de los 0.02 m, por lo tanto dentro de la precisión requerida. El hecho de que este error sea mayor que 0.02 m no significa que la precisión del DGPS sea menor que este valor. Hay que tener en cuenta que este error describe la distancia entre la posición deseada del robot (trayectoria a seguir) y la posición estimada del robot, 7.5 Valoración del algoritmo experimentalmente 171 Tabla 7.5: Paradas del robot (P ), posiciones estimadas del robot con los dos implementaciones de dead-reckoning y errores. P Xdr1(m) Ydr1(m) Xdr2(m) Ydr2(m) ∆dr1(m) ∆dr2(m) 1 -18.3037 -5.6798 -18.3037 -5.6798 0.0055 0.0055 2 -18.5494 -5.7371 -18.5494 -5.7371 0.0732 0.0717 3 -18.8984 -5.8175 -18.8984 -5.8175 0.0703 0.0614 4 -19.2424 -5.8992 -19.2424 -5.8992 0.1316 0.1251 5 -19.5781 -5.9783 -19.5781 -5.9783 0.1493 0.1393 6 -19.9198 -6.0479 -19.9198 -6.0480 0.1585 0.1445 7 -20.2590 -6.1235 -20.2590 -6.1235 0.2137 0.1960 8 -20.5846 -6.2127 -20.5846 -6.2127 0.2327 0.2199 9 -20.9156 -6.2704 -20.9156 -6.2704 0.2504 0.2467 10 -21.2547 -6.3358 -21.2547 -6.3358 0.2908 0.2881 11 -21.5859 -6.4071 -21.5859 -6.4071 0.3293 0.3199 12 -21.9337 -6.5055 -21.9337 -6.5055 0.3363 0.3401 13 -22.2470 -6.5613 -22.2470 -6.5613 0.3695 0.3684 14 -22.5739 -6.6352 -22.5739 -6.6352 0.4367 0.4360 15 -22.8967 -6.7120 -22.8967 -6.7120 0.4534 0.4558 16 -23.2037 -6.7773 -23.2037 -6.7773 0.5208 0.5259 17 -23.5513 -6.8535 -23.5513 -6.8535 0.5441 0.5496 18 -23.9094 -6.9265 -23.9094 -6.9265 0.5468 0.5495 19 -24.2093 -6.9645 -24.2093 -6.9645 0.6337 0.6293 y no entre la posición real y posición estimada. Por tanto, a ese error contribuye los errores de estimación de la posición y los errores de control de posición. Comparando el error absoluto transversal de dr1, dr2 y DGPS con el obtenido con el EKF se aprecia mejoŕıa que presenta el EKF (véase la Figura 7.23). Durante la mayor parte del tiempo en el que transcurre el EXP-4 el error absoluto transversal obtenido con el EKF está por debajo de los 0.02 m. Esto corrobora el buen funcionamiento del seguimiento de trayectoria y el sistema de localización implementado en el robot SILO6. 172 Seguimiento de trayectorias con máquinas caminantes D e s p la z a m ie n to d e l ro b o t Figura 7.13: Puntos de parada del robot (puntos negros), trayetoria a seguir (ĺınea negra) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning -encoders y brújula electromagnética- (azul claro), dead-reckoning -encoders, brújula electromagnética e inclinómetro- (azul oscuro), DGPS (verde) y EKF (rojo). 7.5 Valoración del algoritmo experimentalmente 173 0 2 4 6 8 10 12 14 −1 0 1 2 3 4 5 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a 0 2 4 6 8 10 12 14 16 18 20 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a (a) (b) Figura 7.14: Errores relativos a la distancia recorrida introducidos por la odometria: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. 0 2 4 6 8 10 12 14 −1 −0.5 0 0.5 1 1.5 2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a 0 2 4 6 8 10 12 14 16 18 20 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a (a) (b) Figura 7.15: Errores relativos a la distancia recorrida introducidos por dead-reckoning (dr1): error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. 174 Seguimiento de trayectorias con máquinas caminantes 0 2 4 6 8 10 12 14 −1 −0.5 0 0.5 1 1.5 Paradas E rr or r el at iv a a la d is ta nc ia r ec or rid a 0 2 4 6 8 10 12 14 16 18 20 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a (a) (b) Figura 7.16: Errores relativos a la distancia recorrida introducidos por dead-reckoning (dr2): error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. 0 2 4 6 8 10 12 14 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a 0 2 4 6 8 10 12 14 16 18 20 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2 Paradas E rr or r el at iv o a la d is ta nc ia r ec or rid a (a) (b) Figura 7.17: Errores relativos a la distancia recorrida introducidos por EKF: error relativo absoluto (∆) (negro), error relativo en la coordenada X (∆x) (azul), error relativo en la coordenada Y (∆y) (rojo). (a) Sin control de seguimiento de trayectoria, (b) Con control de seguimiento de trayectoria. 7.5 Valoración del algoritmo experimentalmente 175 1 2 3 4 Figura 7.18: Trayectoria de exploración completa del robot SILO6 para el experimento EXP-4. 176 Seguimiento de trayectorias con máquinas caminantes Origen de la trayectoria Desplazamiento del robot Figura 7.19: Trayetoria a seguir (ĺınea negra) y trayectorias estimadas del robot: odometŕıa (amarillo), dead-reckoning -encoders y brújula electromagnética- (azul claro), dead-reckoning -encoders, brújula electromagnética e inclinómetro- (azul oscuro), DGPS (verde) y EKF (rojo). 7.5 Valoración del algoritmo experimentalmente 177 0 10 20 30 40 50 60 70 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Medidas E rr or a bs ol ut o tr an sv er sa l ( m ) Figura 7.20: Error absoluto transversal introducidos por dead-reckoning (dr1): segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). 0 10 20 30 40 50 60 70 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 Medidas E rr or a bs ol ut o tr an sv er sa l ( m ) Figura 7.21: Error absoluto transversal introducidos por dead-reckoning (dr2): segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). 178 Seguimiento de trayectorias con máquinas caminantes 0 10 20 30 40 50 60 70 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 Medidas E rr or a bs ol ut o tr an sv er sa l ( m ) Figura 7.22: Error absoluto transversal introducidos por dead-reckoning (dr2): segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). 0 10 20 30 40 50 60 70 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 Medidas E rr or a bs ol ut o tr an sv er sa l ( m ) Figura 7.23: Error absoluto transversal introducidos por EKF: segmento1 (azul), segmento2 (rojo), segmento3 (verde) y segmento4 (amarillo). Principales aportaciones El objetivo principal de esta tesis doctoral ha sido el estudio del desplazamiento de una máquina caminante hexápodo siguiendo trayectorias predefinidas sobre terrenos irregulares en presencia de zonas prohibidas. Este es el objetivo global del proyecto “Técnicas de sensorización y control para la detección y localización eficiente de minas antipersonas” DYLEMA (PN I+D+i DPI2001- 1595 y DPI2004-05824) que ha sido el marco de desarrollo de este trabajo. Nuestro objetivo principal supońıa el desplazamiento del CDG del robot sobre una trayectoria previamente especificada (seguimiento de trayectorias), para lo cual se precisaba conocer la posición real del robot en un determinado sistema de coordenadas (localización) y además se requeŕıa mover el robot de tal manera que siguiera la trayectoria evitando pisar sobre las zonas prohibidas definidas por la presencia de alarmas potenciales, de minas u otro tipo de peligro (modo de caminar). Esta información la proporciona un sistema ajeno a los resultados de este trabajo. Tenemos, pues, que el objetivo principal se puede dividir en tres subojetivos: 1. Generación de modos de caminar libres para hexápodos sobre terreno irregular con zonas prohibidas 2. Localización de robots caminantes en exteriores 3. Seguimiento de trayectorias con robots caminantes 1) Generación de modos de caminar Los modos de caminar, presentados en el caṕıtulo 2, se pueden agrupar en: periódicos y no periódicos (Song y Waldron, 1989). En general, los modos de caminar son periódicos, pero los robots caminantes cuentan con la capacidad de decidir el orden en que se producen las transferencias de las patas, dónde se apoyan los pies y los movimientos del cuerpo en función de las caracteŕısticas 180 Principales aportaciones del terreno, de la trayectoria a seguir y del estado de la máquina (modos no periódicos). Estos modos, también conocidos como modos libres, se utilizan para adaptar el robot a las condiciones del terreno y evitar pisar zonas prohibidas premiando la estabilidad durante el movimiento. Sobre la generación de modos de caminar para hexápodos en general se pueden encontrar muchos trabajos en la bibliograf́ıa, no debemos olvidar que fueron los primeros en estudiarse. Algunos de estos trabajos pioneros, que fueron de gran importancia, se definieron para zonas prohibidas, pero no consideraron las irregularidades del terreno, además, sólo se comprobaron en simulación (McGhee y Iswandhi, 1979). Esto nos ha llevado a la generación de nuevos modos de caminar (modos de deriva y modos de giro) que permiten la adaptación a terreno natural con zonas prohibidas (véase el caṕıtulo 3). Estos algoritmos se han diseñado partiendo de reglas heuŕısticas y se han verificado tanto en simulación como experimentalmente. El aspecto de la experimentación no suele aportarse en la bibliograf́ıa cient́ıfica en este campo y, por ello, lo consideramos como una de las aportaciones de esta tesis. Las contribuciones más significativas de este trabajo de investigación en este sub-objetivo se resumen en los siguientes puntos: 1. Se ha definido, implementado y validado experimentalmente un algoritmo para la generación de modos de caminar libres para hexápodos sobre terreno irregular con zonas prohibidas. 2. El modo de caminar, basado en tŕıpodes estables, está caracterizado por no conocer a priori la componente vertical del punto de apoyo del pie. La generación de las trayectorias de las patas se realiza teniendo en cuenta una zona de incertidumbre y se finaliza cuando se detecta realmente el punto de apoyo. 3. El modo de caminar presentado desplaza al robot con un ángulo de deriva. 4. Se han presentado variaciones del modo de caminar para permitir giros tanto de radio no nulo(modos giratorios), como de radio nulo (modos rotatorios). El giro de radio nulo es una rotación alrededor del eje z, (vertical y hacia arriba) del sistema de referencia del cuerpo. 5. Todas las variantes del modo de caminar presentado tienen en cuenta las zonas prohibidas conocidas por el controlador del robot. Un sistema sensorial informa al controlador de la situación de estas zonas. Principales aportaciones 181 6. Se ha propuesto un algoritmo de optimización para la búsqueda de puntos de apoyos para los pies. Hay que destacar como caracteŕısticas del modo de caminar libre desarrollado que puede llegar a comportarse como un modo ondulatorio cuando la situación es la apropiada (ausencia de zonas prohibidas y terreno suave), aún cuando se ha generado a partir de reglas que definen el modo de caminar libre. También se ha constatado la capacidad del algoritmo para maximizar la velocidad de avance y la estabilidad del robot. Además, se ha demostrado que el modo de caminar se puede adaptar a variaciones de la constante de la función de coste para premiar el margen de estabilidad absoluto frente al margen cinemático. Por último, se ha comprobado la adaptación del modo de caminar libre a los cambios instantáneos del ángulo de deriva. 2) Localización de robots caminantes en exteriores La localización, en general, de robots en exteriores ha consumido grandes esfuerzos por parte de la comunidad cient́ıfica; sin embargo, la localización de robots caminantes con sus problemas y particularidades no ha recibido mucha atención y simplemente se ha reportado algún trabajo aislado en esta área (Gassmann et al., 2005). Nuestro trabajo aporta un algoritmo de fusión sensorial que incluye un elevado número de sensores tales como encoders (18), inclinómetros (2), brújula electromagnética y DGPS. Los datos obtenidos con estos sensores se fusionan considerando diversas técnicas como odometŕıa (encoders), dead-reckoning (encoders, inclinómetro y brújula electromagnética) o filtro de Kalman extendido -encoders, inclinómetro, brújula electromagnética y DGPS- (véase el caṕıtulo 6). Todas estas técnicas son habituales en la localización de robots móviles en exteriores, pero la utilización de la información incluyendo la cinemática del mecanismo es exclusiva de robots caminantes y no se hab́ıa estudiado suficientemente. Consideramos que nuestros resultados, validados experimentalmente, suponen una innovación en el campo de la localización en exteriores de robots caminantes. Entre las aportaciones más importantes en este sub-objetivo destacamos: 1. Se ha estimado la posición del robot con odometŕıa a partir de la información incremental de la posición obtenida con el modelo cinemático del mecanismo del robot SILO6. 2. Se han implementado técnicas dead-reckoning en robots caminantes a partir de la información incremental de la posición y de una brújula 182 Principales aportaciones electromagnética para estimar la posición del robot 1. 3. Se ha desarrollado y validado experimentalmente un algoritmo de localización basado en un filtro de Kalman extendido para máquinas caminantes obteniendo una precisión útil, en torno a los ±0.02 m, para el correcto seguimiento de trayectorias predefinidas. 4. Se ha implementado un método, teniendo en cuenta el ángulo de balanceo, para estimar los desplazamientos transversales del cuerpo del robot debido a las holguras de las articulaciones de las patas. Este método cobra una vital importancia cuando el DGPS no funciona correctamente por la pérdida de señal de los satétiles. 5. Cuando el DGPS no recibe señal de los satélites, el algoritmo de localización, basado en técnicas dead-reckoning, toma el error de la estimación de la posición obtenido con el EKF en la estimación anterior, con esto disminuye el error introducido por dead-reckoning hasta ese instante del recorrido. En cuanto a las caracteŕısticas más importantes del sistema de localización implementado en el robot SILO6 conviene resaltar que se ha mejorado la estimación de la posición del robot mediante dead-reckoning al añadir la información obtenida de un inclinómetro para estimar desplazamientos no deseados del robot. Además, se ha comprobado que el sistema de localización se adapta a la información de los sensores disponible en cada instante para evitar cambios bruscos en la estimación de la posición. Finalmente, hay que señalar que los problemas de recepción que puede presentar el DGPS en determinados instantes se subsanan normalmente con la estimación de la posición a partir de dead-reckoning. Esta técnica mantiene los errores de posición por debajo de los requerimientos durante 1 minuto aproximadamente. Si este tiempo se sobrepasa, no se puede garantizar la precisión del desplazamiento y el robot debeŕıa detenerse, hasta que se recupere la señal de los satélites. 3) Seguimiento de trayectoria con robots caminantes El seguimiento de trayectorias se ha obviado en el estudio de robots caminantes a lo largo de estos años, considerándose suficiente desplazar el robot sin seguir una determinada trayectoria. Algunos trabajos han formulado el seguimiento de trayectoria, pero no los han ejecutado en lazo cerrado (Lee 1Aunque todo la experimentación se ha realizado sobre el robot hexápodo SILO6, estás técnicas también se han implementado y validado en el robot cuádrupedo SILO4 (Cobano et al., 2007). Principales aportaciones 183 y Song, 1990); otros investigadores han obtenido resultados de seguimiento de trayectorias, pero sobre terreno propicio y sin zonas prohibidas (Gassmann et al., 2005). En esta tesis doctoral se ha implementado un seguimiento de trayectorias predefinidas, basado en un modo de deriva y un modo de giro, sobre terrenos irregulares y con zonas prohibidas (veáse el caṕıtulo 7). En este aspecto, entendemos que nuestro trabajo es pionero en la comprobación experimental de los algoritmos desarrollados sobre terreno real (natural con zonas prohibidas). Las contribuciones más significativas que aporta esta tesis doctoral en este sub-objetivo son: 1. Se ha desarrollado un algoritmo de control de seguimiento de trayectorias predefinidas con robots caminantes sobre terreno irregular y con zonas prohibidas. 2. Se ha validado en simulación y experimentalmente el seguimiento de trayectorias predefinidas con robots caminantes. 3. El algoritmo intercala un modo de deriva y de giro para corregir continuamente la posición y orientación del robot. 4. El seguimiento de trayectorias implementado es insensible a la presencia de zonas prohibidas y, por lo tanto, el CDG del robot sigue la trayectoria mientras los pies evitan pisar las zonas prohibidas. Como caracteŕısticas principales del algoritmo de control de seguimiento de trayectorias hay que resaltar que se ha demostrado un buen comportamiento en el problema del robot “raptado” (cambios súbitos tanto en la orientación como en la posición del robot) observando como recupera los valores predeterminados de la trayectoria. Además, se ha comprobado experimentalmente el seguimiento de trayectorias en exteriores con errores inferiores a 0.02 m en la dirección transversal a la trayectoria. También se ha validado experimentalmente el modo de giro desarrollado para seguir trayectorias con diferentes orientaciones. El robot puede realizar giros de radio nulo (modos rotatorios) en torno al punto de unión de ambas trayectorias manteniendo la posición respecto al punto de giro. Finalmente, hay que mencionar que el algoritmo para el seguimiento de trayectorias mejora los resultados del sistema de localización (véase el caṕıtulo 7). 184 Principales aportaciones Apéndices Apéndice A Glosario de términos Controlador de trayectoria: el controlador de trayectoria es un módulo de control utilizado para el seguimiento de trayectorias. Este módulo modifica ciertos parámetros del modo de caminar, y es incluso capaz de seleccionar entre varios modos de caminar en función de la trayectoria a seguir. Los parámetros del modo de caminar seleccionados para controlar el movimiento del robot son el ángulo de deriva, α, la velocidad angular de guiñada, ωCG, y la velocidad de avance del cuerpo, VCH . Diagrama de tiempos: el diagrama de tiempos es una herramienta de análisis importante para el estudio de los modos de caminar continuos. Está formado por tantas ĺıneas horizontales como número de patas tiene el robot (véase la Figura 2.5). El tramo de ĺınea continuo indica el tiempo en el que cada pata está en la fase de apoyo. El tramo discontinuo indica la fase de transferencia de la pata en la que ésta se desplaza levantada en busca de un nuevo punto de apoyo. El principio del tramo continuo indica el instante de apoyo del pie y el final indica el instante de levantamiento. Distancia entre centros: La distancia entre centros, P, es la distancia entre los centros de las longitudes de paso de dos patas adyacentes. Px es la distancia entre centros de patas adyacentes colaterales y Px si las patas son contralaterales (véase la Figura 2.2). Exploración completa: La exploración completa de un terreno conlleva precisar estrategias que definan trayectorias que garanticen que el robot móvil reconocerá todo el terreno. Factor de apoyo: El factor de apoyo de la pata i, βi, es la fracción del tiempo de ciclo en el cual la pata i está apoyada en el suelo. Si βi es el mismo para todas 188 Glosario de términos las patas, el modo es regular. Fase relativa: La fase relativa φi de una pata i, es la fracción del tiempo de ciclo que una pata retrasa su instante de apoyo respecto a una pata de referencia. Localización absoluta: La localización absoluta es un tipo de localización que se basa en medidas obtenidas de sensores exteroceptivos. La información que proporcionan estos sensores está referida a un sistema de referencia fijo. Localización del robot: La localización del robot es el conocimiento en todo momento de su posición en un sistema de referencia determinado. Localización relativa: La localización relativa es un tipo de localización que emplea sensores a bordo del robot que proporcionan información incremental de los movimientos del robot. Por lo tanto, la información es relativa respecto a la posición inicial del robot. Longitud de ciclo: La longitud de ciclo, λ, de un modo de caminar es la distancia recorrida por el centro geométrico del cuerpo (CDG) durante un ciclo de locomoción completo. Longitud de paso: La longitud de paso, R, es la distancia que se mueve un pie respecto al cuerpo durante la fase de apoyo. R debe de estar contenido en el volumen de trabajo de la pata definido por Rx y Ry (véase la Figura 2.2). Margen de estabilidad (ME): es la menor distancia desde la proyección vertical sobre el plano horizontal del centro de gravedad de la máquina a los lados del poĺıgono de apoyo (véase la Figura 2.3). Margen de estabilidad delantero y trasero: son las distancias desde la proyección vertical del centro de gravedad de la máquina sobre el poĺıgono de apoyo al lado delantero y trasero, respectivamente, medidas en la dirección de movimiento. Margen de estabilidad longitudinal (MEL): es el menor de los márgenes de estabilidad delantero y trasero (véase la Figura 2.4). Margen de estabilidad longitudinal de modo de caminar o margen de estabilidad de modo de caminar: para un modo periódico es el mı́nimo margen de estabilidad de un ciclo de locomoción completo. Un modo de caminar es estable si MEM ≥ 0, e inestable si MEM < 0. Modo de caminar: Un modo de caminar es la forma o manera de moverse dando pasos, para lo que los animales utilizan patas. En el campo de los robots caminantes un modo de caminar se define como un patrón repetitivo de posicionamiento de los pies. Modos continuos: Los modos continuos están caracterizados por el movimiento constante del cuerpo, producido por las patas en apoyo, mientras las otras patas 189 realizan sus transferencias. Modo circular: Los modos circulares son aquellos que desplazan el cuerpo siguiendo una circunferencia centrada en un determinado punto (véase la Figura 2.7). Modo de deriva: El modo de deriva está caracterizado porque el robot puede desplazarse a lo largo de una recta que forme un determinado ángulo con la dirección del eje longitudinal de su cuerpo. Modos discontinuos: Los modos discontinuos están caracterizados porque el cuerpo permanece inmóvil mientras las patas realizan sus transferencias y sólo se mueve cuando todas las patas están en apoyo. Modo giratorio: Los modos giratorios son aquellos que cambian constantemente la dirección del eje longitudinal del cuerpo del robot. Modos libres: Los modos de caminar están caracterizados porque el robot decide en cada instante qué pata mover y dónde apoyarla en función del entorno que divisan, y se olvidan de la periodicidad en aras de la estabilidad. Modos no periódicos: Los modos no periódicos están caracterizados porque tanto la pata que va a efectuar la transferencia como el punto de apoyo del pie se seleccionan en tiempo real dependiendo de la situación de los elementos del robot (patas y cuerpo) y de las condiciones del terreno. Modo ondulatorio: El modo ondulario es un modo periódico y continuo y es el que se observa en mamı́feros, reptiles e insectos a baja velocidad. Modos periódicos: Los modos periódicos están caracterizados porque las transferencias de patas, los desplazamientos de cuerpo y los puntos de apoyo en el sistema de referencia del cuerpo del robot se repiten ćıclicamente. Modo regular: El modo regular está caracterizado porque el factor de apoyo, βi, es el mismo para todas las patas. Modo rotatorio: Los modos rotatorios son aquellos que giran el cuerpo alrededor del eje z del sistema de referencia del cuerpo (alineado con la aceleración de la gravedad y opuesto a ella) (véase la Figura 2.8). Modo singular: Se considera modo singular aquel en que aparecen simultáneamente apoyos o levantamientos de varios pies. El modo basado en tŕıpodes alternados es singular porque el instante en que se apoyan los pies de un tŕıpode coinciden con el instante de levantamiento de los pies del otro tŕıpode. Modo de tŕıpodes alternados: El modo de tŕıpodes alternados para hexápodos está caracterizado por el apoyo secuencial de dos grupos de tres patas (tŕıpodes) formados por la pata central de un lado y las patas frontal y trasera del otro lado Poĺıgono de apoyo: está definido por las proyecciones verticales sobre un plano 190 Glosario de términos horizontal de las posiciones de los pies en fase de apoyo. Tiempo de ciclo: El tiempo de ciclo de un modo de caminar es el tiempo de duración de un ciclo de locomoción completo. Zonas prohibidas: Las zonas prohibidas son áreas en las que no se pueden apoyar los pies por diferentes causas (elevaciones, agujeros, etc.) Apéndice B Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA B.1. Introducción La detección y localización de minas antipersonas es un grave problema mundial con connotaciones poĺıticas, económicas, medioambientales y humanitarias: afecta a muchos gobiernos que sufren el problema o simplemente lo han creado; el coste de desactivación de los más de 110 millones de minas que se estima están dispuestas para explotar en cualquier instante es elevad́ısimo; las explosiones fortuitas o provocadas contaminan los campos y los inhabilitan para el cultivo; y las mutilaciones y muertes que provocan entre la población civil (normalmente niños) de los páıses que sufren este problema no tienen justificación alguna. A la velocidad de desactivación actual, basada en métodos manuales, el tiempo de desactivación de las minas que han sido sembradas se estima en torno a los 1000 años, asumiendo, obviamente, que no se siembren más. El aumento de la velocidad de limpieza y la disminución de la peligrosidad para los operarios parece que sólo puede garantizarse con el empleo de nuevas tecnoloǵıas que incorporen sensores avanzados, manipuladores eficientes y robots móviles. Este es un ejemplo más de integración de diferentes campos tecnológicos. 192 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA La mejor solución, aunque quizá no sea la más rápida, seŕıa el desarrollo de un sistema completamente automático. Sin embargo, la solución parece estar lejos aún de resultar eficaz. En primer lugar se necesitan sensores y detectores de minas eficientes, estos sensores suelen ser sistemas puntuales que deben moverse sobre los campos minados; esto obliga a algún tipo de sistema de barrido como puede ser un manipulador. Además, el detector debe moverse por campos suficientemente extensos lo que precisa de veh́ıculos capaces de desplazarse en terrenos naturales. En principio, cualquier tipo de robot móvil puede desplazarse con cierta eficiencia sobre campos minados. Los robots con ruedas son los más simples y los más económicos. Los robots con sistemas de orugas son los preferidos en el terreno militar y han demostrado suficientemente su funcionalidad sobre todo tipo de terrenos. Sin embargo, los sistemas de locomoción por patas presentan unas ventajas potenciales muy interesantes para la aplicación que nos ocupa. Por ejemplo: Los robots caminantes sólo requieren un número finito de contactos con el suelo, reduciendo aśı la probabilidad de pisar sobre una mina. Después de detectar una alarma (una mina potencial), un robot con patas tiene una probabilidad mayor de seguir moviéndose que un robot con ruedas u orugas. Las ruedas y las orugas describen trayectorias continuas. Esto supone que ante la presencia de minas tiene que realizar un número determinado de maniobras para esquivar el peligro. Un robot con patas, si ha detectado la posición de una mina puede alterar los puntos de apoyo de los pies y seguir su trayectoria. Esta caracteŕıstica es fundamental cuando se trata de detectar todas las alarmas en un campo y pasar las posiciones de éstas al equipo desactivador. La omnidireccionalidad intŕınseca de los veh́ıculos caminantes les habilita para sortear obstáculos sin necesidad de maniobras de giro que pueden resultar muy peligrosas en otro tipo de veh́ıculos. Los robots caminantes puede desplazarse sobre terreno irregular manteniendo nivelado su cuerpo. Esta caracteŕıstica es muy importante cuando sobre el cuerpo se sitúan sensores y equipos que precisan o conviene que estén nivelados, como ocurre en el caso que estudiamos. Los robots caminantes pueden caminar sobre terreno inclinado (laderas) manteniendo el cuerpo nivelado y, por tanto, sin perjuicio de su estabilidad. B.1 Introducción 193 Un robot con ruedas u orugas, en cambio, se adapta a la inclinación del terreno disminuyendo drásticamente su estabilidad. Los robots caminantes presentan mejor movilidad sobre escaleras, obstáculos y zanjas que los veh́ıculos tradicionales; en resumen, presenta mejores caracteŕısticas para desplazarse sobre terrenos tanto estructurados como no estructurados. Mediante patas se puede caminar sobre terrenos blandos (arenosos, sueltos, embarrados, etc.) y con sensorización adecuada en los pies puede fácilmente detectarse deslizamientos, que es un problema fundamental en la localización de robots en exteriores. Un robot con patas proporciona seis grados de libertad adicionales a los sensores embarcados sin necesidad de cambiar las posiciones de los pies. La utilización de robots caminantes para tareas de desactivación humanitaria se ha estado desarrollando durante los últimos años en dos vertientes: 1. Adaptación de robots existentes 2. Desarrollo de robots espećıficos Entre la primera categoŕıa destaca el TITAN VIII (véase la Figura B.1(a)), un cuadrúpedo desarrollado en el Instituto Tecnológico de Tokio, Japón, como robot caminante de propósito general que ha sido el primer robot caminante adaptado para tareas de desactivación humanitaria (Hirose y Kato, 1998). AMRU-2 es un hexápodo electro-neumático desarrollado por la Universidad Libre de Bruselas y la Academia Real Militar de Bélgica que también ha sido probado como robot para tareas de detección de minas (véase la Figura B.1(d)). El robot RIMHO2 es un robot caminante cuadrúpedo desarrollado en el Instituto de Automática Industrial (IAI-CSIC), es un ejemplo más de robot de propósito general que ha sido probado para esta tarea (véase la Figura B.1(c)). El robot COMET-1 es quizá el primer robot desarrollado espećıficamente para esta tarea (véase la Figura B.1(b)). Este robot es un hexápodo desarrollado por un consorcio japonés que incorpora diferentes sensores y sistemas de localización (Nonami et al., 2000). En la actualidad se está desarrollando la cuarta versión de este robot. Todos estos robots están basados en patas con configuración de insecto, pero también existen otras configuraciones como las estructuras deslizantes 194 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA (a)Robot caminante TITAN VI- II. Instituto Tecnológico de Tokio (Japón). (b)Robot caminante COMET. (c)Robot caminante RIMHO2. Instituto de Automática Indus- trial (IAI-CSIC)(España). (d)Robot caminante AMRU-2. Universidad Libre de Bruselas y Academia Real Militar de Bélgica (Bélgica). Figura B.1: Robots caminantes en tareas de detección de minas antipersonas (Habumuremyi, 1998; Marques et al., 2002). En resumen, hay una gran actividad en el desarrollo de máquinas caminantes para este campo de aplicación espećıfico. B.2 Descripción del sistema y requisitos básicos 195 El IAI-CSIC acredita experiencia en el desarrollo de robots caminantes (Gonzalez de Santos et al., 1995; Grieco et al., 1998; Galvez et al., 2000; Gonzalez de Santos et al., 2000, 2003) y desde 1999 ha estado activo en el área de la detección de minas antipersonas (véase la Figura B.2). El IAI-CSIC tiene experiencia confirmada en el diseño, desarrollo y control de robots caminantes, generación de modos de caminar, adaptación al terreno y teleoperación de robots. Todas estas técnicas están suficientemente maduras para fusionarlas y producir sistemas robotizados eficientes. Siguiendo esta ĺınea, el IAI-CSIC, con financiación del Plan Nacional de I+D+I, ha configurado el sistema DYLEMA (Detección y Localización Eficiente de Minas Antipersonas). El principal objetivo de este proyecto es desarrollar un sistema que integre las tecnoloǵıas más relevantes en el campo de la locomoción caminante y sistemas sensoriales para identificar las necesidades que existen en la aplicación de estas técnicas en desactivación humanitaria. Este apéndice describe las caracteŕısticas básicas del sistema que se ha utilizado como banco de experimentación en esta tesis doctoral. Se mencionarán los distintos subsistemas y se profundizará en la descripción del robot caminante que es el subsistema sobre el que se ha trabajado más directamente. B.2. Descripción del sistema y requisitos básicos El proyecto DYLEMA, como se ha mencionado anteriormente, consiste en un sistema para localización de minas antipersonas que está configurado sobre un robot caminante. El sistema global se descompone en los siguientes subsistemas esquematizados en la Figura B.3: Cabeza sensora Manipulador Localizador Robot caminante Controlador Sistema de alimentación 196 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA Robot caminante RIMHO. Robot escalador REST. Robot caminante SILO4. Robot caminante ROWER. Figura B.2: Robots caminantes y escaladores desarrollados en el IAI-CSIC. B.2 Descripción del sistema y requisitos básicos 197 Figura B.3: Sistema DYLEMA. 1. Cabeza sensora: Este subsistema contiene el detector de minas y elementos adiciones para facilitar su guiado. La cabeza sensora está configurada para detectar alarmas potenciales, pero también permite al controlador mantener la cabeza a una determinada altura sobre el terreno y con una determinada orientación mediante el uso de sensores de infrarrojos. También dispone de sensores de contacto para detectar objetos en el camino de la cabeza sensora. 2. Manipulador: La cabeza sensora es básicamente un detector puntual. Esto significa, obviamente, que sólo puede comprobar la existencia de alarmas en un punto. La eficiencia de este sistema se puede aumentar mediante la práctica de un barrido sistemático que puede llevarse a cabo mediante un manipulador. Por tanto, el sistema DYLEMA dispone de un manipulador que barre una determinada área mientras adapta el sensor a las irregularidades del terreno (altura e inclinación). La sección B.4 justifica que este manipulador debe disponer, al menos, de 5 grados de libertad (GDL). Además, las dimensiones del manipulador deben estar en consonancia con la envergadura del robot caminante y la altura de éste para detectar todos los posibles puntos de apoyo de los pies antes del apoyo real de éstos. 198 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA 3. Localizador: Después de la detección de un objeto sospechoso, el sistema debe registrar en una base de datos la localización exacta de dicho objeto para un posterior estudio y desactivación, si procede, que se realiza por un equipo especializado que complementa las acciones del sistema DYLEMA. Una precisión de ±0.02 m se considera suficiente para esta tarea y puede conseguirse con dispositivos comerciales como un DGPS (Differential Global Positioning Systems). También se utilizan sensores de desplazamiento (encoders) y una brújula electromagnética para computar la posición y orientación del robot. Aunque la precisión conseguida con estos sensores no es la adecuada en esta tarea porque su error aumenta con la distancia recorrida, śı es muy importante su uso en los casos en los que el DGPS presente problemas. 4. Robot caminante: Para las tareas de limpieza de minas se considera de vital importancia disponer de una plataforma móvil que desplace sobre el campo minado todos los sistemas requeridos. En este caso, como ya se ha adelantado, la plataforma se basa en un robot con patas. Los siguientes requisitos han sido el punto de partida de la configuración del robot: El robot caminante se basa en un sistema de seis patas. La sección B.5 justifica esta elección. El robot caminante es suficientemente liviano para ser cargado por dos personas. Este requisito es importante para rescatar al robot de situaciones de bloqueo que pueden encontrarse durante el desarrollo de su tarea. El robot es autónomo desde el punto de vista de la enerǵıa. No se utilizan fuentes de enerǵıa exteriores y por tanto no se utilizan cables umbilicales. El robot es semi-autónomo desde el punto de vista del control. El operario debeŕıa estar incluido en el lazo de control mediante técnicas de teleoperación o control colaborativo. El robot se ha configurando para optimizar la enerǵıa consumida, la movilidad y la estabilidad. Estas son condiciones antagonistas que deben balancearse a través de un cuidadoso diseño. 1. Controlador: El controlador global está dividido en dos computadores (a) el computador embarcado y (b) la estación de operación. El computador B.3 Detección y localización de minas 199 embarcado se encarga de controlar y coordinar los movimientos de las articulaciones tanto del manipulador como de las patas; la comunicación con el DGPS y el detector aśı como la comunicación con la estación de control a través de un enlace de radio ethernet. La estación de operación es un computador remoto encargado de la gestión de tareas del robot caminante y de manejar la base de datos de las alarmas potenciales. 2. Alimentación del sistema: Para configurar un robot autónomo desde el punto de vista energético se dispone de dos soluciones: (a) alimentación de corriente continua a partir de bateŕıas o (b) alimentación de corriente alterna a partir de generadores alimentados con combustible ĺıquido. En principio la segunda opción puede proporcionar más autonomı́a, pero con un sistema más pesado. Las bateŕıas disponen de menor autonomı́a, pero resultan más ligeras. Además, la tecnoloǵıa de las pilas de combustible se está desarrollando rápidamente y se espera que en un plazo relativamente corto se disponga de pilas con autonomı́as elevadas a precios razonables. Por tanto el sistema se ha configurado con motores CC. Tenemos, pues, que el sistema se basa en un robot hexápodo, energéticamente autónomo, que dispone de un manipulador que desplaza un detector comercial de minas antipersonas (barrido). La generación de movimientos y adaptación al terreno del robot caminante son automáticos y el operario participa en el sistema a través de teleoperación y control colaborativo. B.3. Detección y localización de minas Esta sección describe dos subsistemas basados en dispositivos comerciales: la cabeza sensora y el localizador. La cabeza sensora se basa en un detector de minas comercial e integra varios detectores de distancia y de contacto. La sección B.3.1 presenta y justifica el diseño de la cabeza sensora. El localizador es un sistema DGPS comercial. La sección B.3.2 presenta brevemente sus caracteŕısticas y justifica su elección. B.3.1. Cabeza sensora. Actualmente se utilizan diferentes técnicas para la detección de minas. La más sencilla consiste en un detector de metales. Estos sensores son simples, ligeros y de uso muy sencillo. Sin embargo, solo detectan minas que tengan al menos pequeñas 200 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA partes metálicas y resultan ineficaces en la detección de minas plásticas. En este caso deben utilizarse otras tecnoloǵıas como el radar penetrante en tierra (GPR) (Gader et al., 2000), sensores qúımicos (Albert et al., 1999) o sensores olfativos (Rouhi, 1997). Se acepta ampliamente que un sistema de detección eficiente debeŕıa mezclar diferentes tecnoloǵıas. Sin embargo, el proyecto DYLEMA se centra en el desarrollo de los sistemas robotizados para las tareas de detección y localización de minas, y se considera que los detectores de metales son los más simples para la detección de alarmas potenciales. Después de la detección, el sistema debe registrar en la base de datos la posición de la alarma en el sistema de referencia seleccionado para una posterior desactivación. En este proyecto se ha utilizado el sistema comercial Schiebel AN-19/2. Este sistema está homologado por la Armada de E.E. U.U. y la OTAN. Ha sido diseñado para detectar contenidos metálicos muy pequeños y puede detectar minas cuyo único componente metálico sea el detonador. La Figura B.4 muestra este detector y la Figura B.5 muestra el diseño de la cabeza sensora completa. Esta cabeza sensora consta de un soporte que alberga el detector de minas y seis sensores de distancia (infrarrojos) para detectar la distancia y la orientación del sensor respecto al suelo y permite controlar tanto la altura del sensor como la nivelación de éste. Estos sensores están agrupados en parejas que definen el ĺımite superior e inferior de la banda de alturas en la que el sensor trabaja. Esto es lo que permite adaptar el sensor a irregularidades del terreno. Alrededor del detector se dispone de un cinturón de 12 sensores de contacto (flex sensors) que actúa como detector de obstáculos. Están formados por una banda resistiva flexible cuya resistencia eléctrica vaŕıa con la forma. Este cinturón informa al controlador de la posición del objeto contra el que ha chocado la cabeza sensora en su desplazamiento (y por tanto los flex sensors se ha deformado). El sistema permitirá bordear los obstáculos que se encuentran en la trayectoria del robot. B.3.2. Localizador En un sistema de localización de minas es fundamental registrar en la base de datos la posición de todas las alarmas proporcionadas por la cabeza sensora. Esta base de datos debe situarse en el computador de la estación de control. Las técnicas GPS parecen las más adecuadas para estos menesteres porque proporcionan sencillez y precisión. Pero adolecen de algunos inconvenientes: B.3 Detección y localización de minas 201 Figura B.4: Detector de metales. Figura B.5: Cabeza Sensora. 202 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA son extremadamente caras, propensas a errores en áreas con obstáculos tanto naturales como artificiales (árboles, rocas, edificios, etc.), y requieren equipo adicional (una antena y un receptor más) para obtener precisiones centimétricas. Se están empleando sistemas alternativos para los casos en los que el DGPS presenta problemas como son los mencionados anterioremente: encoders y una brújula electromagnética. El sistema DGPS también servirá para calibrar nuevos sistemas de localización futuros. El proyecto DYLEMA requiere la localización de alarmas con una precisión de ±0.02 m. Esta precisión puede conseguirse con la técnica RTK (Real Time Kinematics). Sin embargo requiere una antena GPS adicional que estará situada en la estación de operación. Con estas premisas, el sistema finalmente seleccionado es un equipo TRIMBLE 5700 (véase el apéndice C). B.4. Manipulador Como ya se ha adelantado, el sistema DYLEMA utiliza un detector de minas puntual. Por tanto, es necesario que este sensor lo porte un dispositivo capaz de barrer áreas grandes. El sistema más sencillo puede ser un manipulador diseñado espećıficamente para esta aplicación. Este manipulador requiere 3 GDL para posicionar el sensor en una área 3D, además, requiere 2 GDL adicionales para la adaptación del sensor a las irregularidades del terreno. Como el sensor tiene simetŕıa radial no será necesario ningún GDL adicional para orientar el sensor. Tenemos, pues, que para esta aplicación se requieren al menos 5 GDL. El manipulador debe solamente cargar con la cabeza sensora, por lo tanto, su diseño se ha optimizado para portar esta carga espećıfica. En primer lugar la carga se balancea para mover la muñeca en sus articulaciones de cabeceo y balanceo ±45o. Esto se consigue disponiendo el detector en una posición tal que no requiera ejercerse ningún par cuando esté nivelado. Para esta aplicación la movilidad de la configuración de manipulador RRR resulta adecuada y estando los eslabones principales siempre en un plano perpendicular al suelo (asumiendo que el robot está nivelado) las interacciones con obstáculos disminuyen drásticamente. Las masas se han balanceado adecuadamente para disminuir los pares en las articulaciones. El manipulador completo se muestra en la Figura B.6 y las caracteŕısticas del manipulador se muestran en la tabla B.1. Algunos de los parámetros del manipulador, como la longitud de los eslabones principales, dependen de la envergadura del robot y la altura máxima al suelo que puede alcanzar. La longitud calculada para los B.4 Manipulador 203 Figura B.6: Manipulador de barrido portando la cabeza sensora. eslabones principales es de 0.341 m cada uno, con lo que el manipulador consigue un alcance de 0.682 m. Tabla B.1: Principales caracteŕısticas del manipulador de barrido. Articulación - eslabón Longitud de es- labón (mm) Potencia del motor (watios) Reducción Masa (Kg) 1 60 14 246:1 1.5 2 341 72 357:1 2.1 3 341 26 357:1 1.9 4 200 12 246:1 0.2 5 – 12 246:1 – Total 5.7 204 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA B.5. Configuración del robot caminante Las máquinas caminantes son intŕınsecamente lentas y es conocido que, desde el punto de vista teórico, la velocidad de estos robots depende del número de patas (Waldron et al., 1984). Por tanto, un hexápodo puede alcanzar mayores velocidades que un cuadrúpedo. Además, un hexápodo alcanza su máxima velocidad cuando utiliza un modo de caminar con un factor de apoyo β = 1/2; esto es, cuando utiliza tŕıpodes alternados (Song y Waldron, 1989). Aunque la estabilidad del robot caminante no es óptima cuando se utilizan tŕıpodes alternados, es suficientemente buena (equivalente a la de los cuadrúpedos) y se ha elegido este modo de caminar para paliar en lo posible la baja velocidad intŕınseca de los robots caminantes. B.5.1. Estructura del cuerpo El cuerpo de un robot caminante tiene la misión principal de albergar el sistema de control y servir de soporte de las patas. Por tanto, su tamaño viene determinado por el volumen del sistema de control: computador, amplificadores de potencia, DGPS, bateŕıas, etc., y su forma está influenciada por las medidas de estabilidad que se requieran y otras caracteŕısticas adicionales como el rendimiento energético. Las dimensiones preliminares del robot, cuerpo y patas, aparecen en las tablas B.2 y B.3. Tabla B.2: Caracteŕısticas del cuerpo del robot caminante SILO6. Dimensiones (m) Longitud 0.88 Anchura Frontal/Trasera 0.2 Anchura Centro 0.45 Altura 0.26 Momento de inercia (Kgm2) Ixx 0.99 Iyy 3.11 Izz 0.99 Masa (Kg) 44.34 Velocidad (mm/s) 50 El uso de tŕıpodes alternados como modo de caminar significa que en cualquier instante el robot está soportado por dos patas colaterales no adyacentes y la pata central del lado opuesto. Si todos los pies están situados a la misma distancia B.5 Configuración del robot caminante 205 Tabla B.3: Caracteŕısticas de la pata del robot caminante SILO6. Eslabón 1 2 3 Longitud (mm) 94 250 250 Momento de inercia (Kgm2) Ixx 0.016 0.0027 0.0031 Iyy 0.016 0.0027 0.0031 Izz 0.018 0.0001 0.00003 Masa (Kg) 1 0.5 0.6 Velocidad Pie (mm/s) Transferencia 140 Apoyo 50 del eje longitudinal del cuerpo, la pata central está soportando aproximadamente la mitad del peso del cuerpo mientras que las otras dos patas están soportando cada una aproximadamente la cuarta parte del peso. Si el robot tiene las patas iguales, las patas laterales estarán sobredimensionadas puesto que no deben soportar la misma carga que las patas centrales y para optimizar el sistema las patas debeŕıan ser diferentes (las centrales estaŕıan diseñadas para soportar más carga). Esto tiene el inconveniente de encarecer el diseño y construcción del robot y suele optarse por utilizar todas las patas exactamente iguales lo que ofrece modularidad, facilidad de reposición y, abarata los costes. Una distribución más homogénea de pesos soportados por las patas puede conseguirse si se desplazan las patas centrales alejándolas del eje longitudinal del cuerpo. De este modo se disminuyen las fuerzas soportadas en las patas centrales y se aumenta en las patas laterales con lo que la fuerza máxima que tienen que soportar las patas disminuye sustancialmente (Gonzalez de Santos et al., 2005). La solución final ha consistido en el desplazamiento de las patas centrales 0.125 m frente a las laterales. Con esto se consigue una disminución de las fuerzas en la patas centrales del 14 %. Este diseño determina la forma del cuerpo del robot (véase la Figura B.7). B.5.2. Estructura de la pata Los robots caminantes necesitan configuraciones de patas que proporcionen puntos de contacto con el terreno. Por tanto, un mecanismo de 3 GDL es suficiente para llevar a cabo el movimiento del robot. Las patas deben diseñarse como mecanismos ligeros que soportan cargas elevadas y en muy diferentes posiciones (configuraciones la pata). La configuración mamı́fera (véase la Figura B.7(a)); 206 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA (a) (b) Figura B.7: Robot caminante SILO6: (a) configuración mamı́fero y (b) configuración insecto. es la más eficiente desde el punto de vista energético (requiere pares bajos). Sin embargo, no resulta muy eficiente si analizamos su estabilidad. Las patas de tipo insecto son más eficientes en este caso (véase la Figura B.7(b)); pero, la potencia requerida aumenta considerablemente con esta configuración. La solución final es configurar una pata que pueda proporcionar buena estabilidad y rendimiento energético dependiendo de las necesidades en función, básicamente, del terreno: en terrenos muy irregulares se utilizará la configuración que proporcione buena estabilidad (tipo insecto) y en terrenos propicios, cuando la estabilidad puede garantizarse fácilmente, se impondrá la configuración que mejore el rendimiento energético (tipo mamı́fero). El punto de partida para el diseño de la pata consiste en determinar los pares que se requieren en la configuración más desfavorable: la configuración insecto. Estos pares, para la configuración de cuerpo considerada y con el robot ejecutando un tŕıpode alternado, se representan en la Figura B.8. La gráfica se ha obtenido mediante la simulación de un ciclo de locomoción utilizando el simulador dinámico Yobotics! (Yobotics, 2002). Como es obvio, la articulación 2 debe soportar los pares más elevados. Una forma de disminuir el tamaño y potencia de los actuadores para ejercer un par determinado consiste en utilizar motores en paralelo; esto es, se disponen los actuadores de manera que dos motores simultáneamente proporcionan par a la misma articulación. Estas configuraciones son especialmente ventajosas puesto que los motores actúan simultáneamente sobre dos articulaciones: Por tanto, se tiene tantos grados de libertad como B.5 Configuración del robot caminante 207 Figura B.8: Pares en las articulaciones del robot caminante a lo largo de un ciclo de locomoción empleando tŕıpodes alternados. grados de movilidad y los motores aportan cada uno la mitad del par requerido. Esta disposición funciona de este modo cuando se mueven las articulaciones independientemente. Cuando se mueven simultáneamente ambos motores deben aportar el par requerido por las dos articulaciones y pueden llegar a resultar insuficientes. Estas consideraciones deben tenerse en cuenta en la generación de trayectorias de las patas. Para configurar el sistema paralelo se ha optado por un mecanismo diferencial que se utiliza para mover las articulaciones 2 y 3. La Figura B.9 muestra un esquema del diseño de la pata. Existen dos configuraciones básicas para los pies: pie semiesférico ŕıgido y pie con suela plana y dos articulaciones pasivas. El primero es el más sencillo y con buen funcionamiento en terrenos naturales si el radio de la semiesfera es suficientemente grande. El segundo tiene buenas prestaciones en terrenos blandos, pero resulta más caro y presenta problemas en terrenos muy irregulares por la 208 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA Figura B.9: Configuración de la pata del robot SILO6. tendencia de la suela a voltearse. La solución en nuestro caso se reduce a un pie semiesférico completamente ŕıgido (véase la Figura B.9). B.6. Sistema de control El controlador del sistema está distribuido entre la estación de operación y el controlador del robot (embarcado). Ambos computadores están basados en un bus PC (véase la Figura B.10). La estación de operación está basada en un sistema operativo Windows XP mientras que el controlador del robot utiliza un sistema operativo QNX de tiempo real y multitarea. La comunicación entre la estación de operación y el computador del robot se realiza mediante enlace de radio ethernet. Las siguientes secciones ilustran el hardware y software básico del sistema. B.6.1. Estación de operación La estación de operación está basada en un computador PC situado remotamente respecto al robot caminante y se encarga de la comunicación robot/operador y de la gestión de la base de datos fundamentalmente. Consiste en los siguientes módulos: B.6 Sistema de control 209 1. Interfaz con el operador 2. Gestión de la base de datos de alarmas potenciales 3. Comunicación con el robot móvil. B.6.1.1. Interfaz operario/robot Este módulo es un programa basado en Java que pretende conseguir tres requisitos principales: (a) la monotorización del estado del robot, (b) la teleoperación del robot y (c) la definición de la tarea. El usuario podrá guiar el movimiento del robot remotamente, teniendo información visual de lo que el robot está haciendo en tiempo real. El interfaz hombre-máquina permite también que el usuario defina la tarea a realizar. Esto implica la definición de caracteŕısticas de campos minados (dimensiones del campo, las irregularidades del terreno, etc.), la trayectoria del robot y las estrategias de navegación autónoma. La Figura B.10 muestra cómo es el interfaz hombre-máquina. B.6.1.2. Gestor de la base de datos Cada vez que el robot detecta una alarma se almacena la posición espacial de la detección en una base de datos relacional. De esta forma el usuario puede encontrar la localización de cada alarma encontrada en un campo dado de un pais dado. Las caracteŕısticas del campo y las minas se almacenan también. Esta base de datos permite la posterior tarea de remover la mina. B.6.1.3. Comunicación con el robot caminante La comunicación entre ambos computadores se realiza por medio de un enlace de radio ethernet. Los diferentes sistemas operativos que utilizan los computadores requieren la utilización de sistemas de comunicación compatibles. Uno de estos protocolos es el TCP/IP (Transmission Control Protocol/Internet Protocol) utilizado en todo el mundo para comunicaciones a través de internet. Se ha empleado una arquitectura cliente/servidor donde la estación de operación es el servidor y el robot es el cliente. 210 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA B.6.2. Controlador del robot B.6.2.1. Arquitectura Hardware El controlador del robot se basa en una arquitectura distribuida que consta de un computador PC, una tarjeta de adquisición de datos, y ocho tarjetas de control de motores CC. Cada una de estas tarjetas puede controlar en lazo cerrado (posición y velocidad) hasta tres motores de corriente continua y utiliza tres microcontroladores LM629 de National Semiconductors interconectados a través del bus ISA-PC. Estos controladores disponen de un filtro PID con un generador de trayectorias articulares. Cada microcontrolador comanda un motor CC a través de un amplificador de potencia basado en la técnica PWM. Se emplea una tarjeta analógica, además, para la interconexión de sistemas adicionales (detector de metales, sensores de proximidad, etc. Una tarjeta de comunicaciones radio ethernet permite la comunicación con la estación de operación. La comunicación del DGPS con el computador de control del robot se realiza v́ıa RS-232. El diagrama general del controlador se presenta en la Figura B.11. B.6.2.2. Arquitectura Software El controlador del robot está a cargo de la generación de trayectorias del robot, control del manipulador, procesamiento de señales del detector, comunicaciones con la estación de operación y de la coordinación de los 23 motores sobre el robot caminante (robot y manipulador). Todas estas tareas están distribuidas en una arquitectura jerárquica donde se distingue: Nivel de interfaces hardware: Este nivel contiene los drivers para las articulaciones del robot caminante y el manipulador. Nivel de control de articulaciones: Este nivel se encarga del control individual de las articulaciones del sistema. Nivel de control de pata: En este nivel se coordinan las tres articulaciones de una pata para producir movimientos coordinados Nivel cinemático: Este nivel contiene las transformaciones cinemáticas directa e inversa para realizar trayectoria con los pies. Nivel de control de trayectoria: Este nivel coordina el desplazamiento simultáneo de todas las patas para producir movimientos del robot en ĺınea recta o movimientos circulares. B.6 Sistema de control 211 Figura B.10: Arquitectura del hardware del SILO6. Nivel de estabilidad: Este módulo determina cuándo una configuración de puntos de apoyo es o no estable. Nivel de Generador de modo de caminar: Este nivel genera 212 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA las secuencias de levantamiento y apoyo de patas coordinado con los movimientos del cuerpo para producir el desplazamiento estable del robot. El generador de modos de caminar del robot SILO6 se basa en un modo de tŕıpodes alternados, un modo de giro y un modo de rotación. Nivel de comunicaciones: Este nivel establece la comunicación con la estación de control a través de radio ethernet con un protocolo TCP/IP. Nivel de cinemática del manipulador: Este nivel implementa el modelo cinemático directo e inverso del robot manipulador. Nivel de adquisición de datos: Se encarga de la comunicación y adquisición de datos del detector, sensores de distancia de la cabeza sensora, DGPS, etc. La Figura B.11 muestra esta estructura de control. B.6 Sistema de control 213 Figura B.11: Arquitectura del software del SILO6. 214 Robots caminantes para tareas de detección y localización de minas en tareas humanitarias: El proyecto DYLEMA Apéndice C El Sistema de Posicionamiento Global El sistema GPS (Global Positioning System) o Sistema de Posicionamiento Global es un sistema compuesto por una constelación de 24 satélites situados en órbita a una altura de 20200 km de la Tierra y un receptor. Esta red de satélites se denomina NAVSTAR (NAVigation System and Ranging), es propiedad del gobierno de E.E.U.U. y está gestionado por su Departamento de Defensa (DoD). El sistema GPS permite determinar nuestra posición en cualquier lugar del planeta, a cualquier hora del d́ıa y bajo cualquier condición meteorológica. C.1. Configuración del sistema En el sistema GPS se distinguen tres segmentos o niveles jerárquicos: segmento espacio, segmento control y segmento usuario. Segmento espacio: Está compuesto por 24 satélites (21 primarios y 3 de reserva en órbita) distribuidos en seis planos orbitales inclinados 55o respecto al Ecuador. En cada plano orbital hay cuatro satélites (véase la Figura C.1). El periodo de giro es de 12 horas. Debido a que el periodo de la Tierra es aproximadamente 24 horas (23 horas, 56 minutos) los satélites GPS recorren, en un intervalo de 24 horas, dos veces su órbita espacial y, sin embargo, únicamente habrá completado un viaje alrededor de la superficie terrestre. Todos los satélites poseen un sistema de propulsión que permite mantener la posición de su órbita y el control de estabilidad mediante órdenes de radio desde la Tierra (Sonnenberg, 1998). 216 El Sistema de Posicionamiento Global Figura C.1: Segmento espacio. Segmento de control: Está formado por cinco estaciones monitoras (Hawai (océano Paćıfico este), Kwajalein (océano Paćıfico oeste), Ascensión (Atlántico sur), Diego Garćıa (océano Índico) y Colorado (EE.UU.) (véase la Figura C.2); tres antenas terrestres (Ascensión, Diego Garćıa y Kwajalein) y una estación de control experta situada en Colorado. Segmento usuario: Se incluyen las antenas y los receptores en tierra que proporcionan a los usuarios información acerca de la posición, la velocidad y el tiempo. C.2. Funcionamiento del sistema El funcionamiento del GPS se basa en el conocimiento de la distancia a los satélites en el espacio, por lo tanto se requiere un método que permita averiguar la C.2 Funcionamiento del sistema 217 Figura C.2: Situación de las estaciones monitoras y de la estación experta. separación de dichos satélites. La técnica empleada por el receptor GPS consiste en medir el retardo temporal (tiempo que tarda en llegar una señal de radio emitida desde un satélite) y calcular la distancia multiplicando dicho retardo temporal por la velocidad de propagación de la señal de radio. Para la medición del tiempo de transmisión de la señal de radio se necesita averiguar con exactitud cuándo partió la señal del satélite. Para lograrlo, los relojes de los satélites y los relojes de los receptores GPS están sincronizados de forma que generan el mismo código de forma simultánea. Este código pseudo-aleatorio está formado por un patrón complejo y repetitivo de unos y ceros. De esta forma, como se aprecia en la Figura C.3, se establece una comparación entre la señal de radio recibida y los códigos generados por el receptor GPS, donde un máximo en la correlación indica el tiempo de vuelo de la señal. El empleo de códigos pseudo-aleatorios en los receptores GPS permite un amplio rango de utilidades y un bajo coste de los mismos. Las señales GPS son muy débiles y no se registran por encima del ruido radio de fondo inherente al planeta. Este ruido está formado por una serie de impulsos electrónicos aleatoriamente variables. El código pseudo-aleatorio es muy semejante a dicho ruido pero con una diferencia: su patrón de fluctuaciones es conocido. Puesto que ambas señales son básicamente patrones pseudo-aleatorios, según las leyes de la probabilidad, la mitad de los impulsos coincidirá y la otra mitad no. Sin embargo, si los satélites empiezan a transmitir una serie de impulsos con el mismo patrón que la serie pseudo-aleatoria, estas señales, aunque débiles, tenderán a reforzar el ruido de fondo aleatorio con el mismo patrón empleado en la comparación. Si se desplaza el código pseudo-aleatorio del receptor GPS hasta que se alinee con el de los satélites, el número de coincidencias subirá, es decir, 218 El Sistema de Posicionamiento Global Figura C.3: Comparación entre la señal del receptor GPS y la procedente del satélite. se produce una amplificación de la señal. En otras palabras, el código pseudo- aleatorio permite reconocer una señal débil por lo que los emisores de satélites GPS no han de ser muy potentes y los receptores en tierra captarán la señal con una antena de pequeñas dimensiones. Existen, además, dos razones más que justifican el empleo de códigos pseudo aleatorios. Por un lado, proporciona la forma de que el Departamento de Defensa de los Estados Unidos controle el acceso al sistema de satélites. Aśı, en tiempo de guerra, podrá cambiar el código evitando que el enemigo utilice este sistema, o incluso en tiempo de paz, dotar de cierta exclusividad al sistema. Por otro lado, permite que todos los satélites que integran el sistema empleen la misma banda de frecuencia sin interferirse unos a otros debido a la baja potencia de las transmisiones. Pueden distinguirse tres cadenas distintas de códigos pseudo-aleatorios: el código C/A (Coarse/Adquisition), el código P (precisión Code) y el código Y. El código C/A es el empleado en los receptores civiles, y posee una frecuencia de 1,023 MHz. El código P, por el contrario, posee una frecuencia diez veces superior a la del C/A, esto es, de 10,23 MHz. El código Y, que está encriptado, se env́ıa en lugar del código P cuando el modo de operación AS (disponibilidad selectiva) se encuentra activado. Este modo de operación es el encargado de proteger al sistema contra falsas transmisiones de datos. C.3 Errores del sistema 219 Los satélites transmiten la información en dos frecuencias distintas. Aśı, la frecuencia portadora L1, correspondiente a 1575,42 MHz, transmite código C/A y código P, mientras que la frecuencia portadora L2, correspondiente a 1227,60 MHz, sólo transmite información militar y es modulada en código P. Además, una señal de navegación desde el satélite es transmitida con una frecuencia de 50 Hz en ambas frecuencias portadoras L1 y L2. Las estaciones monitoras establecen una medida de estas señales procedentes de los satélites GPS y la incorporan a los modelos orbitales de cada satélite. Estos modelos computan los datos relativos a las órbitas de los satélites (efemérides), aśı como las correcciones en las desviaciones de los relojes de éstos. Por su parte, la estación de control experta env́ıa la información de efemérides y de las desviaciones de los relojes de los satélites GPS mediante señales de radio. C.3. Errores del sistema Hay diferentes fuentes de error que originan una pérdida de precisión en las medidas efectuadas. Las más importantes son las siguientes: Ionosfera terrestre: es uno de los errores más importantes que afectan a las señales de radio empleadas por los GPS. La ionosfera está formada por una capa de part́ıculas cargadas eléctricamente que modifican la velocidad de las señales de radio cuando la atraviesa. Troposfera terrestre: aqúı residen los fenómenos meteorológicos. Las señales GPS entran en ella después de atravesar la ionosfera. El vapor de agua que existe en la atmósfera afecta a las señales disminuyendo la velocidad de las mismas y, por tanto, aumenta el tiempo en llegar hasta el receptor. Relojes atómicos: estos tienen pequeñas variaciones que pueden afectar a las mediciones. Cálculos matemáticos: se pueden producir pequeñas desviaciones en una operación matemática de cálculo de órbita. Error multisenda: se produce cuando las señales transmitidas desde los satélites rebotan de un lado para otro antes de alcanzar el receptor. Es dif́ıcil corregir estos errores porque dependen del entorno donde se encuentra la antena. 220 El Sistema de Posicionamiento Global Disponibilidad selectiva (S/A): es una forma de degradar intencionada- mente la precisión del GPS. Se introducen errores en los relojes de los satélites y en código privado (Sonnenberg, 1998). Aparece como un cambio continuo en la posición del receptor. Constituye la mayor fuente de error de las señales de radio. C.4. El Sistema de Posicionamiento Global Diferen- cial El sistema DGPS (Differential Global Positioning System) o GPS diferencial es un sistema a través del cual se intenta mejorar la precisión obtenida a través del sistema GPS. El nombre “diferencial” se debe a que el GPS recibe una señal adicional para mejorar la precisión. La señal emitida por los satélites NAVSTAR (GPS) está intencionadamente perturbada con la denominada SA (Selective Availability, Disponibilidad Selectiva) que produce un error de las coordenadas calculadas por un receptor GPS (en navegación) por lo cual se hace necesaria su corrección. La corrección diferencial es el proceso en el cual se sitúa un receptor en un punto conocido, llamado “estación base”, y se utilizan estos datos recogidos para calcular las correcciones en cada instante dadas las coordenadas precisas de la estación. La posición incógnita de otro receptor ubicado en una “estación móvil” (denominado también estación remota o rover) puede mejorar la precisión de su posición aplicando aquellas correcciones. C.5. Elementos del Sistema Diferencial Un dispositivo DGPS necesita de una estación monitora que conozca su posición con una precisión muy alta. Esta estación dispone de: Un receptor GPS. Un microprocesador para calcular los errores del sistema GPS y para generar la estructura del mensaje que se env́ıa a los receptores. Hay un canal de datos unidireccional hacia los receptores, por tanto: Necesita un transmisor (estación monitora). C.5 Elementos del Sistema Diferencial 221 Los usuarios necesitarán un receptor para recibir estos datos. En los mensajes que se env́ıan a los receptores próximos se pueden incluir dos tipos de correcciones: Una corrección directamente aplicada a la posición. Esto tiene el inconveniente de que tanto el usuario como la estación monitora deberán emplear los mismos satélites, pues las correcciones se basan en esos mismos satélites. Una corrección aplicada a las pseudodistancias de cada uno de los satélites visibles. En este caso el usuario podrá hacer la corrección con los 4 satélites de mejor SNR (Signal-to-Noise Ration). Esta corrección es más flexible. El error producido por la disponibilidad selectiva vaŕıa incluso más rápido que la velocidad de transmisión de los datos. Por ello, junto con el mensaje que se env́ıa de correcciones también se env́ıa el tiempo de validez de las correcciones y sus tendencias. Por lo tanto, el receptor deberá hacer algún tipo de interpolación para corregir los errores producidos. El dispositivo DGPS tiene una cobertura de 200 km en torno a la estación base. En esta zona los errores están frecuentemente correlacionados y es en esta zona donde se eliminan los errores del segmento espacial y de control. En cuanto al segmento de los usuarios se eliminan los efectos de la ionosfera y troposfera y el parámetro que más afecta es el ruido del receptor. 222 El Sistema de Posicionamiento Global Apéndice D Geometŕıa del modo de caminar Este apéndice presenta los cálculos necesarios para definir el punto de inicio de transferencia, I(PD, PC , PT , Re) (véase la Definición 3.2), el punto final de transferencia, T (PD, PC , PT , Re) (véase la Definición 3.2), el margen de estabilidad de la transferencia, MEt(PD, PC , PT , Re) (véase la Definición 3.4) y el recorrido de inicio de transferencia, L(PD, PC , PT , Re) (véase la Definición 3.3) a partir de los datos conocidos que son: el tŕıpode que se va a transferir, las posiciones de los pies del tŕıpode de apoyo (PD, PC , PT ) y la distancia que el robot se desplazará durante la transferencia Re. Todos estos conceptos se presentan en la sección 3.3 y la Figura 3.2. Las condiciones que tienen que cumplir los puntos I(PD, PC , PT , Re) y T (PD, PC , PT , Re) son: 1. Los puntos I y T están situados sobre la trayectoria a seguir por el robot (véase la Figura 3.2), luego: Iy = mIx (D.1) Ty = mTx (D.2) donde m es la pendiente de la trayectoria a seguir por el robot, se considera que esta recta pasa por el origen1, y viene determinada por el ángulo de deriva. 1Consideremos que el origen del sistema de referencia del robot coincide con el CDG del robot. 224 Geometŕıa del modo de caminar 2. La distancia entre los puntos I y T es Re (véase la Figura 3.2), luego: R2 e = (Ix − Tx)2 + (Iy − Ty)2 (D.3) donde Re es la distancia que el robot se desplaza durante la transferencia. 3. Las distancias del punto I a la recta PT PC y del punto T a la recta PDPC son iguales (véase la Figura 3.2). Es decir: |Ix(PCy−PTy )+Iy(PTx−PCx )+PTy PCx−PTxPCy |√ (PCy−PTy )2+(PCx−PTx )2 = |Tx(PDy−PCy )+Ty(PCx−PDx )+PCy PDx−PCxPDy |√ (PDy−PCy )2+(PDx−PCx )2 (D.4) Aplicando la condición 1 en la condición 2 se obtiene: (Ix − Tx)2 + (mIx −mTx)2 = R2 e → (Ix − Tx)2 + m2(Ix − Tx)2 = R2 e → (Ix − Tx)2(1 + m2) = R2 e (D.5) Teniendo en cuenta la condición 1, podemos obtener a partir de la condición 3 que: |Ix(PCy−PTy )+mIx(PTx−PCx)+PTy PCx−PTxPCy |√ (PCy−PTy )2+(PCx−PTx)2 = |Tx(PDy−PCy )+mTx(PCx−PDx )+PCy PDx−PCxPDy |√ (PDy−PCy )2+(PDx−PCx )2 (D.6) A partir de (D.6), agrupamos términos de manera que: |IxK3 + K4| K1 = |TxK5 + K6| K2 (D.7) donde K1 = √ (PCy − PTy)2 + (PCx − PTx)2 K2 = √ (PDy − PCy)2 + (PDx − PCx)2 K3 = PCy − PTy + m(PTx − PCx) K4 = PTyPCx − PTxPCy K5 = (PDy − PCy)−m(PCx − PDx) K6 = PCyPDx − PCxPDy (D.8) 225 Por lo tanto podemos expresar Ix como: Ix = ±K1K5 K2K3 Tx ± K1K6 K2K3 ∓ K4 K3 → Ix = Q1Tx + Q2 (D.9) donde Q1 = ±K1K5 K2K3 Q2 = ±K1K6 K2K3 ∓ K4 K3 (D.10) Sustituyendo (D.9) en (D.5) se obtiene: (Q1Tx + Q2 − Tx)2(1 + m2) = R2 e → T 2 x (Q2 1 + 1− 2Q1) + Tx(2Q1Q2 − 2Q2)− R2 e 1+m2 + Q2 2 = 0 → T 2 xG1 + TxG2 + G3 = 0 (D.11) donde G1 = Q2 1 + 1− 2Q1 G2 = 2Q1Q2 − 2Q2 G3 = − R2 e 1+m2 + Q2 2 (D.12) Finalmente, calculamos Tx a partir de (D.11) e Ix a partir de (D.9). Iy y Ty se calculan con (D.1) y (D.2). Conocidos los puntos de inicio, I, y final de transferencia, T , se pueden calcular el margen de estabilidad de la transferencia MEt, y el recorrido de inicio de transferencia, L. El margen de estabilidad de transferencia, MEt, se puede calcular como la distancia del punto I a la recta PT PC o del punto T a la recta PDPC , ya que hemos considerado para los cálculos que ambas son iguales. Por lo tanto se puede calcular con cualquiera de los dos miembros de la igualdad de D.6. En cuanto al recorrido de inicio de transferencia, L, se calcula a partir de la distancia de la posición del CDG del robot al punto de inicio de transferencia, I. L = √ (Ix − CDGx)2 + (Iy − CDGy)2 (D.13) donde CDG coincide con el origen del sistema de referencia del robot. 226 Geometŕıa del modo de caminar Bibliograf́ıa Abbott, E. y Powell, D. (1999). Land-vehicle navigation using GPS. In Proceedings of the IEEE , volume 87, pages 145–162. Acar, E. y Choset, H. (2001). Robust sensor-based coverage of unstructured environments. In Proceedings of the International Conference on Intelligent Robots and Systems, pages 61–68. Albert, K. J., Myrick, M. L., Brown, S. B., Milanovich, F. P., y Walt, D. R. (1999). High-speed fluorescence detection of explosives vapor. Signal Processing, Special Issue on Fuzzy Logic in Signal Processing, pages 308–314. Bai, S., Low, J., y Zielinska, T. (1999). Quadruped free gait generation based on the primary/secondary gait. Robotica, 17, 405–412. Bares y Wettergreen, D. (1999). Dante II: Technical description, results, and lesson learned. IJRR, 18(7), 621–649. Bares, J. y Whittaker, W. L. (1989). Configuration of an autonomous robot for Mars exploration. In World Conference on Robotics Research: The Next Five Years and Beyond , volume 1, pages 37–52. Gaithersburg. Bares, J. E. y Whittaker, W. L. (1993). Configuration of autonomous walkers for extreme terrain. The International Journal of Robotic Research, 12(6), 535–559. Berns, K. (2006). The Walking Machine Catalogue. Available: http://agrosy. informatik.uni-kl.de/wmc/start.php/. Bessonov, A. y Umnov, N. (1973). The analysis of gaits in six-legged vehicles according to their static stability. In Proceedings Of CISM-IFToMM 228 BIBLIOGRAFÍA Symposium on Theory and Practice of Robots and Manipulators, pages 117– 123. Udine, Italy. Borenstein, J. y Feng, L. (1996). Measurement and correction of systematic odometry errors in mobile robots. IEEE Transactions on Robotics and Automation, 12(5), 869–880. Catalbiano, D., Muscato, G., y Russo, F. (2004). Localization and self calibration of a robot for volcano exploration. In Proc. Int. Conf. Climbing and Walking Robots, pages 586–591. New Orleans, LA. Cepolina, F., Moronti, M., Sanguinet, M., Zoppi, M., y Molfino, R. (2006). Roboclimber versus landslides: design and realization of a heavy-duty robot for teleoperated consolidation of rocky walls. IEEE Robotics and Automation Magazine, 13(1), 23–31. Cobano, J. A., Estremera, J., y Gonzalez de Santos, P. (2004). A localization algorithm for outdoor trayectory tracking with legged robots. In Proc. Int. Conf. Climbing and Walking Robots, pages 1119,1126. Madrid, Spain. Cobano, J. A., Gonzalez de Santos, P., y Estremera, J. (2006). A new method to improve the localization of legged robots. In Proc. Int. Conf. Climbing and Walking Robots, pages 485,489. Brussels, Belgium. Cobano, J. A., Estremera, J., y Gonzalez de Santos, P. (2007). Location of legged robots in outdoor environment. Journal of Intelligent and Robotic Systems. Aceptada para publicación. Dean, J., Kindermann, T., Schmitz, J., Schumm, M., y Cruse, H. (1999). Control of walking in the stick insect: from behavior and physiology to modeling. Autonomous Robots, 7, 271–288. Delcomyn, F. (1999). Walking robots and the central and peripheral control of locomation in insects. Autonomous Robots, 7, 259–270. Dellaert, F., Fox, D., Burgard, W., y Thrun, S. (1999). Monte Carlo localization for mobile robots. In Proc. IEEE Int. Conf. Robotics and Automation. Doucet, A., Freitas de, J. F. G., y Gordon, N. J. (2001). Sequential Monte Carlo methods in practice. In Springer . BIBLIOGRAFÍA 229 Eldershaw, C. y Yim, M. (2001). Motion planning of legged vehicles in an unstructured environment. In Proceedings of the International Conference on Robotics and Automation, pages 3383–3389. Seoul, Korea. Estremera, J. y Gonzalez de Santos, P. (2002). Free gaits for quadruped robots over irregular terrain. IJRR, 21(2), 115–130. Estremera, J. y Gonzalez de Santos, P. (2005). Generating continuos free crab gaits for quadruped robots on irregular terrain. IEEE Transactions on Robotics, 21(6), 1067–1076. Estremera, J., Gonzalez de Santos, P., y Lopez-Orozco, J. A. (2005). Neural virtual sensor for terrain adaptation of walking machines. Journal of Robotic Systems, 22(6), 299–311. Fox, D., Burgard, W., y Thrun, S. (1999a). Markov localization for mobile robots in dynamic environments. Journal of Artificial Intelligence Research, 11, 391– 427. Fox, D., Burgard, W., Dellaert, F., y Thrun, S. (1999b). Monte Carlo localization: Efficient position estimation for mobile robots. In Proceedings of the Sixteenth National Conference on Artificial Intelligence. Fukuoka, Y., Kimura, H., y Cohen, A. (2003). Adaptive dynamic walking of a quadrupeed robot on irregular terrain based on biological concepts. The International Journal of Robotic Research, 22(3-4), 187–202. Gader, P. D., Nelson, B., Frigui, H., Vaillette, G., y Keller, J. (2000). Landmine detection in ground penetrating radar using fuzzy logic. Signal Processing, Special Issue on Fuzzy Logic in Signal Processing, 80(6), 1069–1084. Galvez, J. A., Estremera, J., y Gonzalez de Santos, P. (2000). Silo4: a versatile quadruped robot for research in force distribution. In Proceedings of the 3rd International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines, pages 371–383. Professional Engineering Publishers. U.K. Garcia, E. (2002). Optimization of Speed and Stability in Walking Robots. Ph.D. thesis, Polytechnic University of Madrid. 230 BIBLIOGRAFÍA Garcia, E. y Gonzalez de Santos, P. (2004). Mobile robot navigation with complete coverage of unstructured environments. Robotics and Autonomous Systems, 46(4), 195–204. Gassmann, B., Zacharias, F., Zöllner, J. M., y Dillmann, R. (2005). Localization of walking robots. In Proceedings of the 2005 IEEE, International Conference on Robotics and Automation, pages 1483–1488. Barcelona, Spain. Gelb, A., Kasper, J. F. J., Nash, J. R. J., Price, C. F., y Sutherland, A. A. J. (1986). Applied optimal estimation. In The MIT Press. Goel, P., Roumeliotis, S. I., y Sukhatme, G. S. (1999). Robust localization using relative and absolute position estimates. In In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pages 1134–1140. Kongju, Korea. Gonzalez de Santos, P. y Jimenez, M. A. (1995). Path tracking with quadruped walking machines using discontinuous gaits. Computers and Electrical Engineering , 21(6), 383–396. Gonzalez de Santos, P., Armada, M. A., y Jimenez, M. A. (1995). Generation of discontinuous gaits for quadruped walking machines. Journal of Robotics Systems, 12(9), 599–611. Gonzalez de Santos, P., Jimenez, M. A., y Armada, M. A. (2000). Ship building with rower. IEEE Robotics and Automation Magazine, 7(4), 35–43. Gonzalez de Santos, P., Galvez, J. A., Estremera, J., y Garcia, E. (2003). Silo4 - a true walking robot for the comparative study of walking machine techniques. Robotics and Automation Magazine, 10(4), 23–32. Gonzalez de Santos, P., Garcia, E., Cobano, J. A., y Estremera, J. (2004a). Sensor head and scanning manipulator for humanitarian de-minig. In Proceedings of the International Workshop on Robotics and Mechanical Assistance in Humanitarian Demining and Similar Risky Interventions, pages 92–97. Brussels, Belgium. Gonzalez de Santos, P., Garcia, E., Cobano, J. A., y Ramirez, A. (2004b). Silo6: A six-legged robot for humanitarian de-mining tasks. In 10th International Symposium on Robotics and Applications, World Automation Congress. Seville, Spain. BIBLIOGRAFÍA 231 Gonzalez de Santos, P., Garcia, E., Cobano, J. A., y Guardabrazo, T. (2004c). Using walking robots for humanitarian de-mining tasks. In Proceedings of the 35th International Symposium on Robotics, pages 23–26. Paris, France. Gonzalez de Santos, P., Garcia, E., y Estremera, J. (2005). Optimizing leg distribution around the body in walking robots. In Proceedings of the 2005 IEEE International Conference on Robotics and Automation. Gonzalez de Santos, P., Garcia, E., y Estremera, J., editors (2006). QUADRUPEDAL LOCOMOTION: An Introduction to the Control of Four- Legged Robots. Springer, London. Gonzalez de Santos, P., Cobano, J. A., Garcia, E., Estremera, J., y Armada, M. A. (2007). A six-legged-robot-based system for humanitarian demining missions. Mechatronics. Aceptada para publicación. Grewal, M. S. y Andrews, A. P. (1993). Kalman filtering: theory and practice. Prentice-Hall, New Jersey. Grieco, J. C., Prieto, M., Armada, M. A., y Gonzalez de Santos, P. (1998). A six-legged climbing robot for high payloads. In Proceedings of the IEEE International Conference on Control Applications, pages 446–450. Trieste, Italy. Habumuremyi, J. C. (1998). Rational designing of an electropneumatic robot for mine detection. In Proceedings of the 1st International Conference on Climbing and Walking Robots, pages 267–273. Brussels, Belgium. Hirose, S. (1984). A study of design and control of a quadruped walking robot. The International Journal of Robotic Research, 10(2), 113–133. Hirose, S. y Kato, K. (1998). Quadruped walking robot to perform mine detection and removal task. In Proceedings of the 1st International Conference on Climbing and Walking Robots, pages 261–266. Brussels, Belgium. Howard, A., Mataric, M. J., y Sukhatme, G. S. (2003). Putting the ’I’ n ’Team’: an ego-centric approach to cooperative localization. In Proceedings of the IEEE International Conference on Robotics an Automation (ICRA03). Taipei, Taiwan. Jimenez, M. y Gonzalez de Santos, P. (1997). Terrain adaptive gait for walking machines. The International Journal of Robotic Research, 16(3), 320–339. 232 BIBLIOGRAFÍA Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. Transaction of the ASME - Journal of Basic Engineering, pages 35–45. Kugushev, E. I. y Jaroshevskij, V. S. (1975). Problems of selecting a gait for a locomotion robot. In Proceedings of the 4th International Joint Conference on Artificial Intelligence, pages 789–793. Tiblisi, Georgia, URSS. Kumar, V. R. y Waldron, K. J. (1989). A review of research on walking vehicles, pages 243–266. In O. Khatib, J.J. Craig and T. Lozano-Perez, editors. The robotics review. The MIT Press, Cambridge, Massachusetts. Kuroda, Y., Kurosawa, T., Tsuchiya, A., y Kubota, T. (2004). Accurate localization in combination with planet observation and dead reckoning for lunar rover. In Proc. IEEE Int. Conf. Robotics and Automation, pages 2092– 2097. New Orleans, LA. Lee, J. y Song, S. (1990). Path planning and gait of walking machine in an obstacle-strewn environment. Journal of Robotic Systems, 8(6), 801–827. Lewis, M. y Bekey (2002). Gait adaptation in a quadruped robot. Autonomous Robots, 12(3), 301–312. Longo, D., Muscato, G., y Sacco, V. (2002). Localization using fuzzy and kalman filtering data fusion. In Proc. Int. Conf. Climbing and Walking Robots. Paris, France. Madhavan, R., Fregene, K., y Parker, L. E. (2001). Distributed heterogeneous outdoor multirobot localization. In Proceedings of the IEEE International Conference on Robotics an Automation (ICRA01). Mae, Y., Inoue, K., Arai, T., y Nishida, T. (2002). Evaluation of omni-directional mobility of multilegged robots based on error analysis of dead reckoning. In Proc. Int. Conf. Climbing and Walking Robots. Paris, France. Marques, L., Rachkov, M., y Almeida, A. T. (2002). Control system of a demining robot. In Tenth Mediterranean Conference on Control and Automation. Lisbon, Portugal. Maybeck, P. S. (1979). Stochastic models, estimation and control . Academia Press, New Cork, USA. BIBLIOGRAFÍA 233 McGhee, R. B. y Frank, A. A. (1968). On the stability properties of quadruped creeping gaits. Mathematical Bioscience, 3, 331–351. McGhee, R. B. y Iswandhi, G. I. (1979). Adaptive locomotion for a multilegged robot over rough terrain. IEEE Trans. on Systems, Man, and Cybernetics, SMC-9(4), 176–182. Montes, H., Nabulsi, S., y Armada, M. A. (2006). Reliable, built-in, high-accuracy force sensing for legged robots. The International Journal of Robotic Research, 25(9), 931–950. Moors, M., Schneider, F., y Wildermuth, D. (2003). Distributed heterogeneous outdoor multirobot localization. In Proceedings of the 6th International Conference on Climbing and Walking Robots, pages 983–990. Catania, Italy. Negenborn, R. (2003). Robot Localization and Kalman Filters: On finding your position in a noisy world . Ph.D. thesis, Utrecht University. Nonami, K. y Huang, Q. J. (2001). Humanitarian mine detection six-legged walking robot COMET-II with two manipulators. In 4th International Conference on Climbing and Walking Robots, pages 989–996. Karlsruhe, Germany. Nonami, K., Huang, Q. J., Komizo, D., y Uchida, H. (2000). Humanitarian mine detection six-legged walking robot. In 3rd International Conference on Climbing and Walking Robots, pages 861–868. Madrid, Spain. Ojeda, L. y Borenstein, J. (2004). Methods for the reduction of odometry errors in over-constrained mobile robots. Autonomous Robots, 16, 273–286. Orin, D. (1982). Supervisory control of a multilegged robot. The International Journal of Robotic Research, 1(1), 79–91. Pack, D. y Kang, H. (1999). Free gait control for a quadruped walking robot. International Journal of Laboratory Robotics and Automation, 11(2), 71–81. Pal, P. y Jayarajan, K. (1991). Generation of free gaits–a graph search approach. IEEE Transaction on Robotics and Automation, RA-7(3), 299–305. Pal, P. y Kar, D. (2000). Gait optimization through search. The International Journal of Robotic Research, 19(4), 394–408. 234 BIBLIOGRAFÍA Porta, J. M. y Celaya, E. (2001). Efficient gait generation using reinforcement learning. In Proceedings of the 4th International Conference on Climbing and Walking Robots, pages 411–418. Karlsruhe, Germany. Raibert, M. H. (1986). Legged robots that balance. The MIT Press, Cambridge, Massachusetts. Roston, G. P. y Krotkov, E. P. (1991). Dead-reckoning navigation for walking robots. In The Robotic Institute, Carnegie Mellon University. Pittsburgh, Pennsylvania. Rouhi, A. M. (1997). Land mines horrors begging for solutions, chemical. Engineering news, 75(10), 14–22. Roumeliotis, S. I. y Bekey, G. A. (1997). An extended Kalman filter for frequent local and infrequent global sensor data fusion. In Proc. of the SPIE (Sensor Fusion and Decentralized Control in Autonomous Robotic Systems), pages 11– 22. Pittsburgh, Pennsylvania. Song, S. M. y Waldron, K. J. (1989). Machines that walk: The adaptive suspension vehicle. The MIT Press, Cambridge, Massachusetts. Sonnenberg, G. J. (1998). Radar and Electronic Navigation. In Butterworths. Sun, S. S. (1974). A theoretical study of gaits for legged locomotion systems. Ph.D. thesis, The Ohio State University, Columbus, Ohio. Thrun, S. (2002). Particle filters in robotics. In Proceedings of the 18th Annual Conference on Uncertainty in Artificial Intelligence (UAI-02), pages 511–518, San Francisco, CA. Morgan Kaufmann Publishers. Thrun, S., W., B., y Fox, D. (2005). Probabilistic Robotics. The MIT Press, Cambridge, Massachusetts. Thurn, S. (2002). Particle filter in robotics. In Proceedings of Uncertainty in Intelligence Artificial (UAI). Thurn, S., Fox, D., Burgard, W., y Dellaert, F. (2001). Robust Monte Carlo localization for mobile robots. Artificial Intelligence. Volume 128, Number 1-2, pp. 99-141. BIBLIOGRAFÍA 235 Todd, D. J. (1985). Walking machines: An introduction to legged robots. Kogan Page, Ltd. Wald, L. (1998). A European proposal for terms of reference in data fusion. In International Archives of Photogrammetry and Remote Sensing, volume 32. Waldron, K. y McGhee, R. (1986). The adaptive suspension vehicle. IEEE Control Systems, pages 7–12. Waldron, K. J., Vohnout, V. J., Perry, A., y McGhee, R. B. (1984). Configuration design of the adaptive suspension vehicle. The International Journal of Robotic Research, 3(2), 37–48. Welch, G. y Bishop, G. (2004). An introduction to the Kalman filter. In Department of Computer Science, the University of North Carolina. Chapel Hill. Yobotics (2002). Yobotics! Simulation Construction Set: Users Guide. Yobotics Inc., Boston, MA. Available: http://www.yobotics.com/. Zhang, C. y Song, S. (1990). Stability analysis of wave-crab gaits of a quadruped. Journal of Robotic Systems, 7(2), 243–276. Índice General Índice de Figuras Índice de Tablas Capítulo 1Introducción 1.1. Robots caminantes 1.2. Modos de caminar 1.3. Seguimiento de trayectorias 1.4. Localización de robots 1.5. Objetivos de la tesis 1.5.1. Antecedentes 1.5.2. Objetivos globales 1.6. Organización de la memoria Capítulo 2 Modos de caminar 2.1. Generalidades 2.2. Modos ondulatorios 2.3. Modos de deriva 2.4. Modos discontinuos 2.5. Modos libres 2.6. Modos giratorios Capítulo 3 Generación de modos decaminar libres para robots hexápodos 3.1. Introducción 3.2. Planteamiento general del modo de caminar 3.3. Definiciones de estabilidad 3.4. Modelo del terreno 3.5. Defnición del volumen de trabajo 3.6. Defnición de trayectorias de transferencia de las patas 3.6.1. Definición de la trayectoria de un pie en transferencia 3.6.2. Definición de la trayectoria de un trípode en transferencia 3.6.3. Definición de magnitudes 3.7. Planificador de levantamiento de patas 3.8. Movimiento del cuerpo 3.9. Búsqueda de apoyos 3.9.1. Condiciones para los puntos de apoyo 3.9.2. Criterios de optimizaci¶on de los puntos de apoyo 3.9.3. Algoritmo de búsqueda de los puntos de apoyo 3.10. Modos giratorios 3.10.1. Modo circular 3.10.2. Modo rotatorio 3.11. Valoración del algoritmo mediante simulación 3.12. Valoración del algoritmo experimentalmente Capítulo 4 Técnicas de localización en exteriores 4.1. Introducción 4.2. El problema de la localización 4.2.1. Características de la localización 4.2.1.1. Entorno de la tarea 4.2.1.2. Conocimiento sobre el entorno 4.2.1.3. Tipo de tarea 4.2.1.4. Sistema sensorial 4.2.2. Casos típicos de la localización 4.2.3. Información disponible por el robot 4.2.4. Tipos de localización 4.2.4.1. Localización relativa 4.2.4.2. Localización absoluta 4.3. Localización en exteriores 4.3.1. Medidas internas 4.3.1.1. Odometría 4.3.1.2. Sistemas de navegaci¶on inercial: dead-reckoning 4.3.2. Medidas externas 4.3.2.1. Estaciones de transmisión 4.3.2.2. Estimación mediante balizas 4.4. Fusión Sensorial 4.4.1. Filtro de Kalman 4.4.2. Método de Montecarlo 4.4.3. Filtro de partículas 4.5. Localización cooperativa o multi-robot Capítulo 5 Fusión sensorial 5.1. Introducción 5.2. Localización Bayesiana 5.2.1. Modelo de observación 5.2.2. Modelo de movimiento 5.3. Filtro de Kalman 5.3.1. Sistema dinámico lineal 5.3.2. Sistema dinámico no lineal 5.4. Filtro de partículas Capítulo 6 Algoritmo de localización para el Robot SILO 6 6.1. Introducción 6.2. Fase de estimación 6.2.1. Dead-reckoning con encoders y brújula electromagnética. 6.2.2. Dead-reckoning con encoders, brújula electromagnética e inclinómetro. 6.3. Fase de actualización 6.4. Fusión Sensorial mediante un filtro de Kalmanpara localización de robots caminantes en exteriores 6.4.1. Estudio del Sistema Sensorial 6.4.1.1. Sistema de Posicionamiento Global Diferencial 6.4.1.2. Odometría 6.4.1.3. Brújula electromagnética 6.4.1.4. Dead-reckoning 6.4.2. Fusión Sensorial: EKF 6.4.2.1. EKF con encoders, br¶ujula electromagnética y DGPS 6.4.2.2. EKF con encoders, br¶ujula electromagnética, inclinómetro yDGPS Capítulo 7 Seguimiento de trayectoriascon máquinas caminantes 7.1. Introducción 7.2. Defnición de la trayectoria deseada 7.3. Control de seguimiento de trayectoria 7.3.1. Control azimutal del robot 7.3.2. Control de posici¶on horizontal del robot 7.3.3. Control de velocidad de avance del robot 7.3.4. Control de cambio de trayectoria y selecci¶on de modo decaminar 7.4. Valoración del algoritmo mediante simulación 7.5. Valoración del algoritmo experimentalmente 7.5.1. EXP-3: Seguimiento de una trayectoria rectilínea 7.5.2. EXP-4: Trayectoria de exploración completa Principales aportaciones Apéndices Apéndice A Glosario de términos Apéndice B Robots caminantes para tareasde detección y localización deminas en tareas humanitarias:El proyecto DYLEMA B.1. Introducción B.2. Descripción del sistema y requisitos básicos B.3. Detección y localización de minas B.3.1. Cabeza sensora. B.3.2. Localizador B.4. Manipulador B.5. Configuración del robot caminante B.5.1. Estructura del cuerpo B.5.2. Estructura de la pata B.6. Sistema de control B.6.1. Estación de operación B.6.1.1. Interfaz operario/robot B.6.1.2. Gestor de la base de datos B.6.1.3. Comunicación con el robot caminante B.6.2. Controlador del robot B.6.2.1. Arquitectura Hardware B.6.2.2. Arquitectura Software Apéndice C El Sistema de Posicionamiento Global C.1. Configuración del sistema C.2. Funcionamiento del sistema C.3. Errores del sistema C.4. El Sistema de Posicionamiento Global Diferencial C.5. Elementos del Sistema Diferencial Apéndice D Geometría del modo de caminar Bibliografía