Post on 18-Jul-2022
DISEÑO DE LEYES DE CONTROL BASADAS EN LA TEORÍA DE LA ESTABILIDADDE ATRACTORES, APLICADAS EN ROBOTS AGENTCOOP JUGADORES DE
FÚTBOL.
JOAQUÍN ANDRÉS VALDERRAMA RINCÓN
ANDRÉS ORLANDO PAEZ MELO
PONTIFICIA UNIVERSIDAD JAVERIANA
FACULTAD DE INGENIERÍA
DEPARTAMENTO DE INGENIERÍA ELECTRÓNICA
BOGOTÁ, D.C.
2013
1
DISEÑO DE LEYES DE CONTROL BASADAS EN LA TEORÍA DE LA ESTABILIDADDE ATRACTORES, APLICADAS EN ROBOTS AGENTCOOP JUGADORES DE
FÚTBOL.
JOAQUÍN ANDRÉS VALDERRAMA RINCÓN
ANDRÉS ORLANDO PAEZ MELO
Trabajo de Profundización
Maestría en Ingeniería Electrónica
Director:Ing. DIEGO ALEJANDRO PATIÑO Ph.D.
PONTIFICIA UNIVERSIDAD JAVERIANAFACULTAD DE INGENIERÍA
DEPARTAMENTO DE INGENIERÍA ELECTRÓNICABOGOTÁ, D.C.
2013
2
PONTIFICIA UNIVERSIDAD JAVERIANA
FACULTAD DE INGENIERÍA
DEPARTAMENTO DE INGENIERÍA ELECTRÓNICA
RECTOR MAGNÍFICO R.P. JOAQUÍN SÁNCHEZ S.J.
DECANO ACADÉMICO ING. JORGE LUIS SANCHEZ TELLEZ, M.Ed. M.Sc.
DECANO DEL MEDIO UNIVERSITARIO R.P. SERGIO BERNAL S.J.
DIRECTOR DE MAESTRÍA ING. CESAR LEONARDO NINO BARRERA, Ph.D.
DIRECTOR DEL PROYECTO ING. DIEGO ALEJANDRO PATIÑO GUEVARA, Ph.D.
3
Nota de advertencia
"La universidad no se hace responsable de los conceptos emitidos por sus alumnos en sus proyectos degrado.
Sólo velará porque no se publique nada contrario al dogma y la moral católica y porque los trabajos nocontengan ataques o polémicas puramente personales. Antes bien, que se vea en ellos el anhelo de buscar laverdad y la justicia".
Artículo 23 de la Resolución No. 13, del 6 de julio de 1946, por la cual se reglamenta lo concerniente a
Tesis y Exámenes de Grado en la Pontificia Universidad Javeriana.
4
Introducción
Dentro de las ramas de la robótica móvil uno de los principales campos de estudio consiste en encontrarla autonomía en dichos dispositivos, en la cual existe un mecanismo de toma de desición para ejecutar unatarea; y una de las aplicaciones con un campo de acción de gran acogida es la Robocopa, una liga en la quese promueve la competencia entre robots autónomos resaltando la investigación y los avances en inteligenciaartificial. Una de las disciplinas de dicho campeonato es la SSL (robots de pequeño tamaño), la cual se centraen el problema de la cooperación multi-agente y el control, en un entorno altamente dinámico con un sistemahíbrido centralizado, dentro de un robot móvil que cumpla con las medidas establecidas por la competencia [1].
Para que un equipo de robots pueda jugar fútbol, se requiere la combinación de tecnologías que permitanalcanzar este objetivo: diseño de un conjunto de robots autónomos, implementación de sistemas multitareas, pla-neación de estrategias en tiempo real, acondicionamiento de un grupo de sensores que perciban el movimientopara realimentar un criterio de decisión, entre otros. De acuerdo a las reglas establecidas por la Robocopa, enesta liga se debe incluir tres componentes: Un sistema de visión, para tener un panorama del escenario de juego;un sistema de inteligencia artificial, para recibir la información del sistema de visión y tomar los criterios de de-cisión para cada robot del equipo; y el equipo de robots, quienes se encargarán de ejecutar la tarea programaday las decisiones tomadas por el sistema de inteligencia artificial.
En la actualidad, los grupos de investigación SIDRE y SIRP, de la Pontificia Universidad Javeriana deBogotá, dentro del proyecto AgentCoop [2] (Agentes Cooperativos: Cooperación en Sistemas multiagentesaplicados a robótica móvil), cuentan con una modalidad aplicada a robots jugadores de fútbol, específicamenteadaptados para la SSL.
Dentro del proyecto se dispone de todo el hardware necesario para la liga SSL. Se tiene un procesador centralen un computador que se encuentra enlazado a un sistema de visión externo, dentro del cual funciona todo elsistema de inteligencia artificial, y cinco robots de movimiento holonómico, con las medidas reglamentarias dela liga.
En este sistema se identificaron tres grandes problemas en la implementación de toda la arquitectura: Enprimer lugar, se presentan problemas en cuanto al movimiento del robot, observándose falencias en el controlde los motores, respondiendo de forma inadecuada a una velocidad indicada, y presentando inconsistenciasen los valores leidos por los sensores asociados (encoders). En segundo lugar, se identificó cómo el canal decomunicaciones inalámbrico resultaba insuficiente para los datos que se estaban enviando desde el computador(inteligencia artificial) hacia los robots, generando retrasos en la llegada de paquetes y comportamientos nodeseados en los mismos. En tercer lugar, se observa la falta de autonomía de los robots para sus movimientos,siendo el computador el encargado de calcular las velocidades de móvil y de motor para cada uno de los robots,en cualquier tayectoria establecida.
Para esta situación, en este proyecto se busca implementar un control de posición embebido dentro de cada
5
uno de los robots, y dejar que sea él mismo el encargado de realizar todas las tareas correspondientes a su ubi-cación y movimiento. Esto permite un mejor aprovechamiento de los recursos del robot, aumenta la autonomíadel mismo, disminuye la cantidad de datos que viajan por el canal inalámbrico, y reduce la carga del sistemade inteligencia artificial. Adicional a esto, se implementa un rediseño total del software en todos los nivelesdel robot, que se adapte de forma adecuada a los últimos cambios de hardware implementados por el grupo detrabajo, y se obtenga una respuesta adecuada conforme a los requerimientos.
Para lograr dichos objetivos, el desarrollo de este proyecto consiste en:
Diseñar e implementar un control de posición embebido en cada robot, que permita obtener velocidadesde móvil y calcular velocidades de motor.
Adaptar el protocolo de comunicación inalámbrica para recibir y diferenciar coordenadas de posiciónpara trayectorias.
Diseñar un sistema de localización que integre las medidas de posición relativa calculadas por la odome-tría del robot y la posición global obtenidas desde el sistema de visión artificial.
Diseñar e implementar un control de velocidad para cada uno de los motores, realimentado por una lecturarediseñada de los encoders.
Adecuar el protocolo de comunicaciones interno del robot (backplane) , que permita enviar valores develocidad de motor, y recibir datos de los encoder de los motores.
Rediseñar el software del robot en todos los niveles, mejorando considerablemente el desempeño y adap-tación de los mismos al nuevo diseño de hardware.
6
Índice general
1. Fundamentos teóricos 131.1. Métodos de posicionamiento de los robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.1.1. Métodos de Posición Absoluta . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131.1.1.1. Reconocimiento de Marcadores Artificiales . . . . . . . . . . . . . . . . . . 131.1.1.2. Coincidencia de Modelos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131.1.1.3. Visión Artificial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.1.2. Métodos de Posición Relativa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.1.2.1. Navegación Inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.1.2.2. Odometría . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.2. Técnicas de control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.2.1. Técnicas para Controlar Robots Holonómicos . . . . . . . . . . . . . . . . . . . . . . . 151.2.2. Control Digital . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.2.2.1. Sistemas de Control Digital . . . . . . . . . . . . . . . . . . . . . . . . . . . 161.2.2.2. Windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
1.3. Comunicación UART . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171.4. FreeRTOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.4.1. Tareas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.4.2. Colas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.4.3. Interrupciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2. Arquitectura general del proyecto Agentcoop 202.1. Módulo de visión artificial (Capa perceptual) . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.2. Módulo de estrategia de acción cooperativa (Capa Estratégica) . . . . . . . . . . . . . . . . . . 222.3. Robots (Capa Comportamental) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3. Arquitectura general de los robots 243.1. Descripción física . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.1.1. Diagrama de bloques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.1.1.1. Procesador maestro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.1.1.2. Módulo de comunicaciones inalámbricas . . . . . . . . . . . . . . . . . . . . 263.1.1.3. Módulo de control de pelota (Kicker) . . . . . . . . . . . . . . . . . . . . . . 273.1.1.4. Módulo de control de los motores . . . . . . . . . . . . . . . . . . . . . . . . 27
3.1.1.4.1. Procesador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
7
3.1.1.4.2. Motoreductor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.1.1.4.3. Encoder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.1.1.4.4. Rueda . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.2. Descripción lógica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.2.1. Módulo Maestro del Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.2.1.1. Bloque de Comunicación (Backplane) . . . . . . . . . . . . . . . . . . . . . 303.2.1.2. Bloque Comunicación (Inalámbrica) . . . . . . . . . . . . . . . . . . . . . . 303.2.1.3. Bloque Cinemática del Robot . . . . . . . . . . . . . . . . . . . . . . . . . . 313.2.1.4. Bloque Odometría . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.2.1.5. Bloque Control de Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.2.2. Módulo control de motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.2.2.1. Bloque de Comunicación . . . . . . . . . . . . . . . . . . . . . . . . . . . . 333.2.2.2. Bloque de Lectura de Pulsos . . . . . . . . . . . . . . . . . . . . . . . . . . 333.2.2.3. Bloque de Control de velocidad . . . . . . . . . . . . . . . . . . . . . . . . . 333.2.2.4. Bloque de PWM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4. Control de velocidad 354.1. Descripción funcionamiento procesador de los motores . . . . . . . . . . . . . . . . . . . . . . 354.2. Cálculo de tiempos del módulo de control de motores . . . . . . . . . . . . . . . . . . . . . . . 37
4.2.1. Bloque de lectura de pulsos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.2.2. Bloque de control de velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.2.3. Bloque de comunicaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.2.3.1. Timer para control de errores . . . . . . . . . . . . . . . . . . . . . . . . . . 414.3. Bloque de Lectura de Pulsos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.4. Cálculos de PWM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.5. Descripción del control de velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.5.1. Modelamiento de los Motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 454.5.2. Cálculos del control de velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 464.5.3. Anti-windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5. Control de posición 515.1. Cinemática del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 515.2. Odometría . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 535.3. Cálculos control de posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.3.1. Cambio del eje de referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 585.3.2. Descripción Control de posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 605.3.3. Saturador de velocidad de motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . 615.3.4. Control de posición implementado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.3.4.1. Pruebas y resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 645.4. Descripción diagrama de tareas procesador maestro . . . . . . . . . . . . . . . . . . . . . . . . 69
5.4.1. Interrupción UART . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 715.4.2. Interrupción CommSlaves . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 715.4.3. Tarea Control de Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
8
5.4.4. Tarea Comunicación con esclavos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 725.4.5. Tarea Control Localización . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 735.4.6. Tarea Recepción de datos Xbee . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 735.4.7. Tarea Envio de datos Xbee . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
6. Comunicación 756.1. Comunicación (Backplane) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 756.2. Descripción del protocolo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
6.2.1. Trama de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 776.2.2. Cálculo de tiempos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 796.2.3. Detección de errores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
6.3. Comunicación inalámbrica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
7. Conclusiones. 82
Bibliografía 82
8. Anexos 858.1. Soccer Small Leage (SSL) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 858.2. Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
8.2.1. Módulo de Visión Artificial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 878.2.2. Módulo de Estrategia de acción cooperativa . . . . . . . . . . . . . . . . . . . . . . . . 888.2.3. Módulo Maestro del Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 888.2.4. Módulo de Control de los Motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
9
Índice de figuras
1.1. Encoder Óptico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.2. Diagrama de Bloques de un sistema de control digital [3] . . . . . . . . . . . . . . . . . . . . . 161.3. Controlador con anti-windup donde la salida del actuador es estimada desde un modelo mate-
mático. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171.4. Trama típica de datos UART . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171.5. Estados de una tarea [4] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.6. Estructuras de datos FIFO y LIFO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191.7. Interrupciones en RTOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.1. Imagen interacción entre módulos robocopa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.2. Descripción interconexión general de módulos, proyecto Agentcoop . . . . . . . . . . . . . . . 21
3.1. Modelo CAD del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.2. Parámetros físicos del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.3. Diagrama de módulos del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.4. Módulo control de los motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.5. Rueda omnidireccional . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.6. Módulo Maestro del Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.7. Topología de comunicación para cuatro esclavos . . . . . . . . . . . . . . . . . . . . . . . . . . 303.8. Esquema de control de posición dentro del robot . . . . . . . . . . . . . . . . . . . . . . . . . . 323.9. Módulo control de motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.1. Diagrama de tiempos bloque control motores . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.2. Tiempo para periodo de comunicación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 404.3. Salida del encoder, máxima velocidad @12 Voltios . . . . . . . . . . . . . . . . . . . . . . . . 424.4. Diagrama de Bloques Control con Antiwindup . . . . . . . . . . . . . . . . . . . . . . . . . . . 454.5. Respuestas de dos Motores a Diferentes Entradas . . . . . . . . . . . . . . . . . . . . . . . . . 464.6. Motor Retroalimentado con el Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 474.7. Diagrama de Control de Velocidad Motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494.8. Simulación de Diferentes Entradas Paso a un Motor con Control . . . . . . . . . . . . . . . . . 494.9. Comparación respuesta control motores a) paso 40. b) paso 60. c) paso 80. d) paso 100. Donde
t es X 10ms. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.1. Esquema geométrico robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
10
5.2. Radio de la rueda . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 545.3. Diagrama de Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 575.4. Diagrama de bloques del control de posición del robot. . . . . . . . . . . . . . . . . . . . . . . 585.5. Ejes de referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 595.6. Rotación eje de coordenadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 605.7. Respuesta control optimo entrada paso a) grafica x-y. b) grafica t-x. c) grafica t-y. d) grafica t-θ . 615.8. Diagrama de la saturación de velocidades de motor . . . . . . . . . . . . . . . . . . . . . . . . 625.9. Diagrama de la saturación velocidades de motor en matlab . . . . . . . . . . . . . . . . . . . . 625.10. Control de pocicon Implementado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 635.11. Respuesta control Implementado a) grafica x-y. b) grafica t-x. c) grafica t-y. d) grafica t-teta.
Donde t es tiempo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 645.12. Resultados en X - Prueba desplazamiento en X . . . . . . . . . . . . . . . . . . . . . . . . . . 655.13. Resultados en Y - Prueba desplazamiento en Y . . . . . . . . . . . . . . . . . . . . . . . . . . 655.14. Resultados en X - Prueba desplazamiento en Theta . . . . . . . . . . . . . . . . . . . . . . . . 665.15. Resultados - Prueba desplazamiento distancias cortas . . . . . . . . . . . . . . . . . . . . . . . 675.16. Resultados - Prueba desplazamiento distancias largas . . . . . . . . . . . . . . . . . . . . . . . 685.17. Pruebas de UBMark para odometría . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 695.18. Diagrama de tareas módulo maestro del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
6.1. Número de esclavos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 756.2. Trama de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 766.3. Trama de datos de comunicación del módulo maestro del robot . . . . . . . . . . . . . . . . . . 776.4. Comunicación para 3 esclavos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 786.5. Detección de errores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 806.6. Trama de datos de comunicación inalámbrica . . . . . . . . . . . . . . . . . . . . . . . . . . . 806.7. Encabezado comunicación inalámbrica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
8.1. Medidas máximas reglamentarias liga SSL [5] . . . . . . . . . . . . . . . . . . . . . . . . . . . 858.2. Medidas reglamentarias del Arco, liga SSL . . . . . . . . . . . . . . . . . . . . . . . . . . . . 868.3. Dimensiones, en milímetros, de la cancha para la liga SSL . . . . . . . . . . . . . . . . . . . . 868.4. Patrón de colores para identificación de robots en liga SSL . . . . . . . . . . . . . . . . . . . . 878.5. Medidas reglamentarias para el patrón de colores . . . . . . . . . . . . . . . . . . . . . . . . . 87
11
Índice de cuadros
2.1. Capas de la arquitectura del proyecto Agentcoop . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.1. Tabla de parámetros físicos del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.1. Jerarquía de interrupciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364.2. Tiempos módulo de control de motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.3. Cuadro de tiempos del bloque de lectura de pulsos . . . . . . . . . . . . . . . . . . . . . . . . . 384.4. Cuadro de tiempos del bloque de control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394.5. Tiempo de las instrucciones del bloque de comunicaciones . . . . . . . . . . . . . . . . . . . . 394.6. Tiempos del peor de los casos en el inicio de la comunicación . . . . . . . . . . . . . . . . . . 40
5.1. Parámetros para prueba de desplazamiento X . . . . . . . . . . . . . . . . . . . . . . . . . . . 645.2. Parámetros para prueba de desplazamiento Y . . . . . . . . . . . . . . . . . . . . . . . . . . . 655.3. Parámetros para prueba de desplazamiento en Theta . . . . . . . . . . . . . . . . . . . . . . . . 665.4. Parámetros para prueba de desplazamiento en distancias cortas . . . . . . . . . . . . . . . . . . 665.5. Parámetros para prueba de desplazamiento en distancias largas . . . . . . . . . . . . . . . . . . 675.6. Tabla de prioridades de las tareas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 695.7. Tabla de colas del procesador maestro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 705.8. Tabla detalles interrupción UART . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 715.9. Tabla detalles interrupción CommSlaves . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 715.10. Tabla detalles tarea control de posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 725.11. Tabla detalles tarea comunicacion con esclavos . . . . . . . . . . . . . . . . . . . . . . . . . . 725.12. Tabla detalles tarea localizacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 735.13. Tabla detalles tarea localizacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 735.14. Tabla detalles tarea envío de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
12
Capítulo 1
Fundamentos teóricos
1.1. Métodos de posicionamiento de los robots
El conocimiento exacto acerca de la posición de un vehículo es un problema fundamental en aplicaciones derobótica móvil. Aunque una gran variedad de sistemas, sensores y técnicas para el posicionamiento de un robotmóvil han sido desarrollados, no hay realmente una verdadera solución definida para el problema. En generalexisten dos tipos de algoritmos para la estimación de posición: el primer tipo son los estimadores de posiciónabsoluta, que incluyen cualquier dispositivo con el que se tenga una observación externa de la posición; eneste grupo es común encontrar reconocimiento de marcadores artificiales, sistemas de posicionamiento global(GPS), coincidencia de modelos y elementos de visión artificial. El segundo tipo son las medidas de posiciónrelativa, que comprenden odometría, y sistemas de navegación inercial [6]. Usualmente al menos dos métodos,consistentes en uno de cada tipo, son utilizados de forma combinada [7].
1.1.1. Métodos de Posición Absoluta
1.1.1.1. Reconocimiento de Marcadores Artificiales
La principal característica de este método es la ubicación de marcadores artificiales dentro del medio am-biente y en lugares conocidos. La principal ventaja de los marcadores artificiales es que son diseñados para quepuedan ser detectados fácilmente por el robot a pesar de las condiciones adversas que pueda tener el entornodentro del cual se encuentra el mismo. Adicionalmente, se debe garantizar que tres o más marcadores siempreestén a la vista del robot para permitir una estimación correcta de la posición. El posicionamiento por mediode marcadores artificiales tiene la ventaja de que los errores de posición están limitados. Sin embargo, sus prin-cipales desventajas consisten en una alta complejidad computacional y la necesidad de modificar el ambienteexterno [8].
1.1.1.2. Coincidencia de Modelos
En este método la información adquirida a partir de los sensores que posee el robot móvil es comparada conun modelo de mapa de su entorno. Si las características obtenidas de los sensores del robot coinciden con lasalmacenadas en el modelo del mapa, entonces se puede estimar la posición absoluta del robot móvil. Los mapasusados para la navegación se clasifican en dos grupos: mapas geométricos y mapas topológicos. Los mapas
13
geométricos representan el entorno en un sistema global de coordenadas, mientras que el mapa topológico lorepresenta como un grafo compuesto por nodos y arcos [8].
1.1.1.3. Visión Artificial
Esta técnica de localización emplea una cámara de video con la cual a partir del procesamiento de la imagenobtenida se hace un reconocimiento de distintos patrones para hallar la posición del robot móvil y su orientación,así como los objetos que están en el entorno [8].
La técnica de visión artificial está sujeta a errores debido a cambios en la luz cuando se capturan las imáge-nes, generando errores del orden de centímetros de una medida a otra. Esto hace que existan cambios bruscosen la referencia de posición, generando acumulación de errores cada vez que esto sucede y causando que losrobots patinen. Debido a que la cámara se encuentra en un ambiente abierto, la luz del ambiente hace que estoscambios de luz se produzcan en todo momento.
Esto, combinado con los errores debido a la perspectiva de la cámara a diferentes distancias dentro delcampo, además de errores por ruidos como son los movimientos, hace que no sea un sistema suficiente paraobtener una posición confiable del robot.
1.1.2. Métodos de Posición Relativa
Son los métodos que utilizan medidas de posición que dependen de componentes y medidas dentro delrobot.
1.1.2.1. Navegación Inercial
La navegación inercial emplea giróscopos y algunas veces acelerómetros para medir la tasa de rotacióny aceleración. Las medidas son combinadas una o dos veces para así generar el valor de la posición. Lossistemas de navegación inercial también tienen la ventaja de ser independientes - aislados. Por otro lado, lainformación recolectada por los sensores inerciales se desvía con el tiempo; por lo tanto, los sensores inercialesson inapropiados cuando se desean obtener medidas exactas de posición durante largos períodos de tiempo.
1.1.2.2. Odometría
La odometría es la técnica utilizada por la mayoría de sistemas robóticos para navegar. Los encoders ópticosdel sistema dan una medida de la rotación del motor, como se observa en la Figura 1.1. La información de laposición es obtenida integrando dichas señales. Tiene la ventaja de ser totalmente auto contenida en el robot, ysiempre es capaz de proveer al vehículo con una estimación de su posición [7]. La principal desventaja es quela posición y los errores de orientación crecen sin límite alguno, debido a que las mediciones que son obtenidasdeben ser integradas una o dos veces para obtener la información de la posición. El mayor inconveniente queexiste es que, debido a la integración, cualquier error de medición en alguna pequeña constante puede llevar aun error significativo en la información correspondiente [9].
14
Figura 1.1: Encoder Óptico
Los sistemas de localización descritos, fueron usados en el proyecto de robótica cooperativa AgentCoop, asícomo en proyectos anteriores del grupo SIDRe. De acuerdo a estas experiencias, el grupo seleccionó la técnicade localización por odometría, actualizada periódicamente, para eliminar su error acumulado, con la técnica devisión artificial. Esta combinación de sistemas de localización se debió hacer debido al error acumulado de laodometría, que generaría una desubicación del robot después de un instante de tiempo funcionando con unaúnica medida.
1.2. Técnicas de control
Como el robot es una máquina compuesta de un sistema complejo que interactúa con su entorno por mediode movimientos que son controlados por un procesador, requiere realizar movimientos de una forma segura,rápida y precisa [10]. Por lo tanto es necesario modelar matemáticamente el sistema y establecer estrategias decontrol continuas o discretas, que se describirán a continuación.
1.2.1. Técnicas para Controlar Robots Holonómicos
Para realizar el control de la posición del robot holonómico se utilizan diferentes métodos entre los cuales seencuentran: el control PID, control difuso y control óptimo lineal. Con el control PID frecuentemente se controlala velocidad, la posición y el ángulo en el robot. Para controlar la velocidad se utiliza un control proporcionalP, para el ángulo se utiliza un proporcional derivativo PD, y para la posición se utiliza un proporcional integralPI garantizando que no haya error cuando está en estado estable para alguna posición. Para tener un mejordesempeño, los parámetros de los controladores varían automáticamente según el error que se genera con laentrada. Para implementar el control difuso es necesario en primer lugar linealizar el sistema, después se obtieneel sistema aumentado en forma discreta y por último se diseña un regulador óptimo para el sistema de matricesaumentados determinando la ganancia de la matriz solucionando la ecuación de Ricati asociada, utilizandoaproximaciones numéricas [11].
1.2.2. Control Digital
En los últimos años se ha incrementado sustancialmente el uso de controladores digitales implementadosen procesadores. Estos controladores digitales se utilizan para lograr el desempeño óptimo como productividadmáxima o mínimo consumo de energía. [3]
15
1.2.2.1. Sistemas de Control Digital
En la Figura 1.2 se muestra un diagrama de bloques de un sistema de control digital que presenta la configu-ración del esquema de control básico. Es de resaltar que en el diagrama mostrado se discretiza la señal de errormediante un módulo de muestreo y retención y la señal del controlador es calculada por la computadora digital;por útlimo dicha señal se convierte mediante la cascada de un módulo digital - análogo y un módulo retenedor,generalmente de orden cero. [3].
Figura 1.2: Diagrama de Bloques de un sistema de control digital [3]
1.2.2.2. Windup
Aunque muchos aspectos de un sistema de control pueden ser entendidos basados en teoría lineal, algunosefectos no lineales deben tenerse en cuenta en prácticamente todos los controladores. Windup es un fenómeno,el cual es causado por las interacciones de la acción integral y las saturaciones. Todos los actuadores tienenlimitaciones y para un sistema de control con un amplio rango de condiciones operando, puede ocurrir que lavariable de control alcance los límites del actuador. Cuando esto pasa el lazo de retroalimentación es roto yel sistema corre como un lazo abierto porque el actuador permanecerá en su límite independientemente de lasalida del proceso. Si un controlador con acción integradora es usado, el error continuará siendo integrado. Estosignifica que el término integral puede convertirse en un valor muy grande o, coloquialmente, esto es "Wind up".Entonces es requerido que el error tenga signo opuesto por un largo periodo antes de que las cosas retornen a lanormalidad. La consecuencia es que cualquier controlador con acción integral puede dar grandes transicionescuando el actuador se satura.
Al implementar técnicas de antiwindup en un controlador cuya estructura PID es paralela, como se muestraen la figura 1.3, se debe incorporar al modelo matemático tal cual como se indica, para que dicha inclusión seaefectiva en la sección de saturación del actuador.
16
Figura 1.3: Controlador con anti-windup donde la salida del actuador es estimada desde un modelo matemático.
En este proyectó se implementó un control digital de velocidad PI y un control de pocisión compuesto por3 sistemas de control independientes: posición en el eje X, posición en el eje Y y posición angular, cada uno deellos PID.
La implementación de un sistema de antiwindup se debe principalmente al sistema de alimentación utilizadoen los robots. Cuando las baterías se descargan y no alcanzan el nivel máximo de voltaje con el cual se trabaja(12V), los motores no alcanzan a girar al máximo teórico establecido, lo que produiría una saturación del sistemade control.
1.3. Comunicación UART
Dentro del proyecto Agentcoop, la comunicación inalámbrica se realiza mediante el envío de tramas con elprotocolo UART, a través de un canal inalámbrico compuesto por dispositivos XBEE en modo de transmisiónserial transparente (AT), el cual funciona como un reemplazo de una línea serial convencional. [12].
El formato de datos de trasmisión y recepción de UART se muestra en la figura 1.4, la cual usualmenteincluye un bit de inicio, bits de datos, bit de paridad, bit de stop, y un estado de inactividad. Este formato detrama de datos se toma desde el bit de inicio, hasta los bits de parada, con el fin de lograr la sincronización decaracteres. UART tiene un registro de configuración interna, en el cual el usuario puede configurar la cantidadde bits de datos, decidir si incluir o no un bit de paridad, asi como el tipo de paridad y bits de parada.[13, 14]
Figura 1.4: Trama típica de datos UART
1.4. FreeRTOS
Todo el software del contol de posición de los robots Agentcoop se programa sobre un sistema operativo entiempo real, en este caso el FreeRTOS versión 7.
17
Un sistema en tiempo real es aquel en el que la corrección del resultado depende tanto de su validez lógicacomo del instante en que se produce. En este tipo de sistemas es tan importante la validez lógica de los cálculoscomo la validez temporal de los mismos. Para garantizar el tiempo de respuesta, un sistema en tiempo realnecesita no solo velocidad de respuesta, sino determinismo. Es decir, el tiempo de ejecución de los programasde tiempo real ha de estar acotado en el caso más desfavorable, de forma que se garantice que se cumpliránsiempre las restricciones temporales [15].
En los sistemas en tiempo real es muy frecuente encontrar un software dividido en varios niveles jerárquicos,como la adquisisión de datos y actuadores, algoritmos de control PID, algoritmos de supervisión, registro dedatos, entre otros. Por otro lado, no todas las tareas son igual de urgentes. Normalmente cuanto más bajo estánen la jerarquía mencionada anteriormente, las tareas son más prioritarias. En definitiva, el paralelismo es unacaracterística inherente a los sistemas en tiempo real.
Salvo que se disponga de un sistema con varios procesadores, la ejecución en paralelo se consigue mediantela sucesión rápida de ejecuciones parciales de las tareas. Es decir, el sistema ejecuta una tarea (tarea 1) duranteun periodo de tiempo, otra tarea (tarea 2) durante otro periodo de tiempo y así sucesivamente. Como el sistemaconmuta rápidamente entre tarea y tarea, desde fuera da la impresión que se están ejecutando todas las tareas enparalelo [16].
1.4.1. Tareas
El bloque básico de un programa basado en un sistema operativo de tiempo real es la tarea. Una tarea no esmás que una función, aunque esta función obviamente puede llamar a otras funciones. La única condición queha de cumplir una función para convertirse en una tarea es que no termine nunca. La tarea se inicializa medianteuna llamada al sistema operativo en tiempo real, especificándose en dicha llamada la prioridad de la tarea, lamemoria que necesita, la función que la implanta (denominada punto de entrada), etc.
La figura 1.5 muestra el diagrama de estados de ejecución de una tarea. [16]
Figura 1.5: Estados de una tarea [4]
1.4.2. Colas
Una cola es una estructura de datos, caracterizada por ser una secuencia de elementos en la que la operaciónde inserción se realiza por un extremo y la operación de extracción por el otro. También se le llama estructuraFIFO (del inglés First In First Out), figura 1.6, debido a que el primer elemento en entrar será también el primeroen salir [17].
18
Figura 1.6: Estructuras de datos FIFO y LIFO
Por defecto las colas en el FreeRTOS son estructuras de datos de tipo FIFO, sin embargo, es posible emu-lar un comportamiento de tipo LIFO (del inglés Last In First Out), donde el último elemento en entrar en lacola es el primero que se procesa. Esto es particularmente útil cuando se tiene un generador de datos que esmás veloz que la tarea los procesa, y el último dato enviado es el que debe entrar en la tarea para tener elcomportamiento deseado. Para esto se utiliza la función xQueueSendToFront desde una tarea en ejecución, oxQueueSendToFrontFromISR en el caso de encontrarse dentro de una interrupción.
1.4.3. Interrupciones
Las interrupciones en el sistema operativo de tiempo real funcionan de la misma forma como lo haríanen el procesador sin este sistema, sin embargo, se deben definir como funciones especiales, de manera que elsistema se entere cuando se esta ejecutando una. Se debe tener en cuenta, además, que no se utilizan las mismasfunciones que en una tarea habitual. Existen funciones especiales para su llamada desde las interrupciones.Existen funciones especiales para ser llamadas desde la rutina de interrupción que se diferencian de las normalesen que no conmutan las tareas. Por ejemplo, existe una función para enviar un mensaje a una cola y otra paramandarlo desde la rutina de interrupción. En estos casos sigue siendo necesario llamar a una función paraindicar la salida de la interrupción de forma que el sistema operativo ejecute el planificador y conmute a otratarea de mayor prioridad que haya cambiado al estado “lista” como consecuencia de la ejecución de la rutina deinterrupción. [17].
Las interrupciones tienen una mayor prioridad que cualquier tarea del sistema, por lo cual, cuando se pre-senta una interrupción, el sistema pasa de inmediato a atenderla, y hasta que no termine su gestión, no vuelveal sistema de tareas normal. La figura 1.7, muestra el comportamiento del sistema operativo cuando se presentauna interrupción.
Figura 1.7: Interrupciones en RTOS
19
Capítulo 2
Arquitectura general del proyectoAgentcoop
El proyecto Agentcoop aplicado a robots jugadores de fútbol, cuenta con una arquitectura de capas que seadaptan a las reglas generales de juego establecidas para la liga SSL. Consta de tres capas principales, de lascuales su desarrollo se hace de manera independiente dentro del proyecto, pero de cuya interacción y acopledepende el éxito del sistema. La primera capa es una capa comportamental asociada al sistema de control delrobot, la cual, en el caso de este proyecto, se encuentra completamente embebida dentro de cada uno de losrobots que hacen parte del equipo. La segunda capa es una capa perceptual, representada en este caso por unsistema de visión artificial externo, con acceso a la totalidad del entorno de trabajo de los robots, el cual parael proyecto consta de un campo de fútbol con las medidas reglamentarias de la liga SSL. Por último, la capa
estratégica, la cual es ejecutada en su totalidad por un computador externo, cuya función es la de realizar laacción cooperativa que controla el conjunto de robots, como se muestra en la figura 2.1.
Figura 2.1: Imagen interacción entre módulos robocopa
20
En la tabla 2.1 se muestran las principales características de cada una de las capas de las que se compone laarquitectura del proyecto.
Capa Harware deEjecución Entrada Salida
Percepción Externa Visiónartificial
Imagen del entorno de losrobots
Coordenadas de los elementosen el entorno
Estrategia de accióncooperativa Computador Coordenadas de los
elementos en el entornoCoordenadas de ubicación y
de atractor de cada robot
Comportamiento Robot Coordenadas de ubicación yde atractor de cada robot Movimiento
Cuadro 2.1: Capas de la arquitectura del proyecto Agentcoop
En la figura 2.2 se muestra la gráficamente la función principal de cada uno de los módulos, y la intercone-xión de los mismos.
Figura 2.2: Descripción interconexión general de módulos, proyecto Agentcoop
21
2.1. Módulo de visión artificial (Capa perceptual)
El objetivo de este módulo es obtener la información del ambiente en que se desarrolla el partido, principal-mente la posición de los robots y la pelota, mediante una o varias cámaras de video colocadas a 4m por encimade la cancha. El sistema de visión es la única fuente de retroalimentación en toda la arquitectura, en lo referentea la interacción de los robots con su entorno. Este sistema debe ser lo suficientemente robusto para compensarlas principales fuentes de error, como son la variabilidad en la intensidad de la luz o la proyección de sombrasen la cancha de juego. Si la información que proporciona el sistema de visión es incorrecta, el rendimientoglobal del equipo se verá seriamente afectado, puesto que la capa de Estrategía de acción cooperativa y la capa
comportamental dependen de la confiabilidad de los datos obtenidos en este módulo.Las principales tareas del sistema de visión son:
Capturar la imagen de la cancha, video en tiempo real, a través de 2 cámaras que se ubican 4m por encimade la cancha
Reconocer y diferenciar la ubicación de cada robot por medio de los parches de colores reglamentariosde la liga, que tienen los robots en la cubierta, ver anexo 8.1.
Transmitir la información al Módulo de estrategia de acción cooperativa.
Adaptarse a las diferentes condiciones de luz y sombras.
2.2. Módulo de estrategia de acción cooperativa (Capa Estratégica)
Este módulo es el encargado de la inteligencia conjunta de juego, su objetivo central es el de tomar decisio-nes en cuanto a la estrategia a utilizar dependiendo del rol asignado a cada robot, con base en la información delsistema de visión (posición y orientación de los robots). Su implementación se desarrolla enteramente por soft-ware, basado en una arquitectura de capas multiagentes MRCC (Multi-Resolution Cooperative Control AgentArchitecture) [18], implementado sobre la plataforma multiagentes BESA [19], dentro de las cuales se defineny ejecutan cada una de las acciones cooperativas convenidas.
Estas acciones cooperativas se basan en una asignación dinámica de roles, donde un rol se puede definircomo un conjunto de objetivos, habilidades y recursos que permiten a un agente la realización de tareas dentrode una arquitectura de acción cooperativa [18]. En este caso, un rol puede ser visto como el conjunto de ele-mentos asignados a un robot, que le permiten alcanzar uno o más objetivos individuales, encaminados hacia elcumplimiento de los objetivos del equipo. En el modelo de 4 capas utilizado, hay dos tipos diferentes de roles asaber, el rol estructural y el cooperativo [20], sobre los cuales se basa la asignación general de funciones en elequipo, y según los cuales se obtiene la salida final del módulo, una serie de puntos atractores actualizados entiempo real que servirán de referencia a los robots para ejecutar las acciones deseadas.
Las principales funciones de este módulo son:
Servir como puente entre el sistema de visión artificial y los robots, con el fin de poder comunicarle acada uno de ellos las coordenadas de su ubicación actual y de atractor.
Realizar los cálculos del punto atractor de cada móvil que deba efectuar un movimiento requerido.
22
Definir las acciones apropiadas para cada uno de los robots en el campo, y transformar cada comporta-miento en puntos atractores para comunicar a cada uno de los móviles.
2.3. Robots (Capa Comportamental)
Los robots componen la capa comportamental del proyecto. Son los dispositivos móviles encargados derecibir, procesar y llevar a cabo las órdenes enviadas desde el módulo de estrategía de acción cooperativa.Se trata de mecanismos autónomos independientes, capaces llegar a una posición determinada de su entornocon base en la información de localización obtenida como resultado de la visión artificial externa (medida deposición absoulta) y la odometría (medida de posición relativa).
Consiste de móviles holonómicos de tres ruedas impulsadas por motores DC, con las medidas reglamentariaspara la categoría SSL de la robocopa, ver anexo 8.1.
Las principales funciones de los robots son:
Determinar las coordenadas de su posición actual, con base en la información recibida por el sistema devisión artificial y los cálculos de odometria realizados.
Realizar un control de posición con base en la información de las coordenadas de localización obtenidasy las coordenadas de punto atractor recibidas por el módulo de estrategia de acción cooperativa.
Realizar un control de velocidad para cada uno de sus motores, con el fin de obtener un movimientouniforme en cada una de las ruedas al aplicarles la misma referencia de velocidad .
23
Capítulo 3
Arquitectura general de los robots
En este capítulo se presenta una descripción general de los robots. En la primera sección se muestran loscomponentes físicos y una breve descripción de cada uno y sus funciones, de acuerdo con los últimos cambiosrealizados por parte del grupo de Agentcoop hasta la fecha . En la segunda sección se muestra la descripciónlógica del diseño propuesto en este proyecto, representado en un diagrama de bloques, con una breve descripciónde cada uno de ellos.
3.1. Descripción física
Los robots del proyecto Agentcoop, consisten de móviles holonómicos de tres ruedas, impulsados por mo-tores DC conectados a cada una de ellas, alimentados por medio de 8 baterías AA conectadas en serie. Cuentancon un mecanismo de manejo de pelota basado en un rodillo giratorio y un mecanismo de disparo basado en elcambio del campo magnético de un solenoide.
La base y la tapa de la estructura se forman por medio de dos placas de acero inoxidable, sostenidas portres columnas metálicas separadas uniformemente, formando una estructura dentro de la cual se ubican: latarjeta que contiene el procesador maestro, las tarjetas que contienen los módulos de control de cada uno delos motores, y los motores. En la parte exterior, se ubican las tres ruedas omnidireccionales que permiten elmovimiento del robot. La figura 3.1 muestra un modelo en CAD del robot [20].
Figura 3.1: Modelo CAD del robot
El movimiento y el desempeño de los robots dependen directamente de sus principales parámetros físicos,
24
tales como el radio del robot, el radio de cada una de la ruedas, la distancia desde el centro del robot hacia cadauna de las ruedas, la posición angular de los motores, y el peso de la estructura en su totalidad. En la figura 3.2se muestran los principales parámetros físicos analizados en el robot.
R
RT
r
M1
M2
M3
Figura 3.2: Parámetros físicos del robot
Donde:
M1→Motor número 1.M2→Motor número 2.M3→Motor número 3.R→ Radio del robot.RT → Distancia desde el centro del robot hasta las ruedas.r→ Radio de las llantas.θ1→ Ángulo del motor 1.θ2→ Ángulo del motor 2.θ3→ Ángulo del motor 3.
En la tabla 3.1 se muestran los valores de los parámetros del robot indicados en la figura 3.2.
Medida ValorRadio del Robot (R) 8cm
Radio de las ruedas (r) 2,5cmDistancia desde el centro del robot hasta la rueda (RT ) 9cm
Ángulo motor 1 (θ1) 60◦
Ángulo motor 2 (θ2) 180◦
Ángulo motor 3 (θ3) 300◦
Cuadro 3.1: Tabla de parámetros físicos del robot
25
3.1.1. Diagrama de bloques
Cada robot se compone de un procesador maestro, encargado de las funciones de localización y controlde posición del móvil, un módulo de control para cada uno de los motores presentes en el robot y un módulode comunicaciones inalámbrico, encargado de permitir el intercambio de información entre los móviles y elmódulo de estrategia de acción cooperativa. La figura 3.3 muestra la forma como se conectan los módulosentre si.
Figura 3.3: Diagrama de módulos del robot
Se trata de mecanismos autónomos independientes, capaces llegar a una posición determinada de su entornocon base en la información obtenida a partir de una fusión multisensorial de su ubicación.
3.1.1.1. Procesador maestro
Es el encargado de llevar a cabo todas las tareas lógicas del robot. Su función principal es la de controlar laposición del móvil de acuerdo con los datos de ubicación obtenidos a partir de una fusión multisensorial y laposición del atractor requerido. La validación de la localización del robot viene dada por la información de unsistema de visión artificial externo y los datos calculados por la odometría embebida dentro del mismo.
También es el encargado de cumplir funciones de comunicación con el módulo de estrategia de acción
cooperativa y con el módulo de control de los motores.
3.1.1.2. Módulo de comunicaciones inalámbricas
Es el encargado de permitir una comunicación inalámbrica bidireccional en cada robot con el módulo de
estrategia de acción cooperativa. Consiste en un módulo XBEE, cuya función principal es la de recibir losdatos envidos desde el procesador maestro mediante UART (Universal Asynchronous Receiver-Transmitter), y
26
permitir la modulación digital en 2,4GHz para ser transmitidos al nodo coordinador de la red, en este caso elbloque de comunicaciones del módulo de estrategia de acción cooperativa.
3.1.1.3. Módulo de control de pelota (Kicker)
Este módulo es el encargado de permitir un control de pelota y realizar las acciones de pateo del robot. Sufunción principal es la de detectar cuando el robot entra en contacto con la pelota, y dependiendo del comandoindicado por el procesador maestro, llevar a cabo la tarea de controlar la bola, patear un pase a otro de los robotso realizar un disparo al arco en busca de un gol.
Se basa en un solenoide controlado por un circuito de elevación de voltaje. El núcleo del solenoide contienedos materiales: acero y aluminio. Su ubicación se encuentra en la parte frontal del robot, en el centro de suestructura.
Actualmente este bloque se encuentra en una etapa de desarrollo, fuera de los alcances del proyecto actual yforma parte de un proyecto de investigación que adelanta de manera independiente al desarrollo de este trabajode grado.
3.1.1.4. Módulo de control de los motores
Figura 3.4: Módulo control de los motores
3.1.1.4.1. Procesador Es el encargado de llevar a cabo todas las funciones lógicas del módulo de control delos motores. Su función principal es la de realizar un control de la velocidad del motor. En este caso consiste enun microcontrolador de bajo costo y consumo de potencia, con capacidad de generar señales con modulaciónde ancho de pulso que permitan variar la velocidad del motor y controlar la dirección de su giro; manejar unprotocolo de comunicación para tener una transferencia bidireccional de datos con el maestro del robot, y recibiry procesar las señales enviadas desde el encoder que permitan tener una medición del giro del motor.
3.1.1.4.2. Motoreductor El motoreductor es un dispositivo mecánico ubicado en la salida del motor, cuyoobjetivo es el de realizar una reducción en la velocidad del mismo, por medio de un juego de engranajes que
27
sirven de puente entre el mecanismo de giro del motor y su salida fisica. Esta reducción implica una transfor-mación de cierta cantidad de vueltas del motor, en una vuelta efectiva en la salida del mismo. En este caso, losengranajes estan dispuestos de forma tal que, cada 10 vueltas completas del motor, se tiene una vuelta efectivaen la salida del motoreductor. De esta forma, se tiene que:
1 GiroE f ectivo−→ 10×GiroMotor (3.1)
lo cual implica que:
Ve f ectiva =Vmotor
10(3.2)
Es decir, con el motoreductor se reduce la velocidad efectiva del motor a su décima parte.
3.1.1.4.3. Encoder El encoder es el dispositivo encargado de permitir una realimentación de la velocidadefectiva del motor hacía el procesador, con el fin de cerrar el lazo de control. La salida del encoder consiste endos señales cuadradas cuyo periodo es proporcional a la velocidad del motor, y cuyo ángulo de desfase indica ladirección del giro. Cada 100 pulsos del encoder (flancos de subida de la señal de salida) indica que el motor hadado un giro completo, pero debido a la presencia del motoreductor, se necesitan 10 giros del motor para tenerun giro efectivo en la rueda. De esta forma, un giro efectivo equivale a un total de:
10giromotor/giroefectivo×100pulsos/giromotor = 1000pulsos/giroefectivo (3.3)
Es decir, se tiene un giro efectivo de la rueda cada 1000 pulsos contados en la señal del encoder.
3.1.1.4.4. Rueda El tipo de desplazamiento que se debe considerar para un robot móvil está sumamenterelacionado con el tipo y la geometría de las ruedas. Para el desplazamiento de este tipo de robots se escogió elde tipo omnidireccional, porque brinda una completa maniobrabilidad. Los robots con ruedas omnidireccionalespueden moverse en cualquier dirección y en cualquier momento sin requerir una orientación específica para eldesplazamiento del mismo, permitiendo el comportamiento holonómico deseado. Este tipo de desplazamientorequiere ruedas que se puedan mover en más de una dirección, como la mostrada en la figura 3.5.
Figura 3.5: Rueda omnidireccional
28
3.2. Descripción lógica
3.2.1. Módulo Maestro del Robot
Figura 3.6: Módulo Maestro del Robot
Este módulo se encarga del manejo lógico de las funciones del robot, dentro de las que se pueden enumerar:
1. Control de posición del móvil.
2. Comunicación bidireccional con el Módulo de Estrategia Cooperativa.
3. Comunicación bidireccional con los Módulos de Control de los motores.
4. Cálculo de la odometría del móvil.
5. Fusión multisensorial de posición por medio de un filtro de Kalman no lineal.
6. Conversión y adecuación de velocidades de móvil a velocidades de motor.
29
La figura 3.6 muestra un diagrama de bloques que representa la organización interna del procesador maestro
del robot.
3.2.1.1. Bloque de Comunicación (Backplane)
Este bloque se encarga de permitir una comunicación sincrónica bidireccional con los módulos de controlde los motores. Su función es la de enviar los datos de velocidad a cada uno de los motores del robot, y recibirlos datos de la lectura de pulsos generados por los encoders de los mismos.
Se basa en una configuración Maestro - Esclavo interconectado a través de un bus serial, donde cada unode los módulos de control de los motores hace la función de esclavo, y el módulo maestro del robot funcionacomo maestro de la red, siendo el único habilitado para iniciar una comunicación y el encargado de generar laseñal de reloj para la sincronía de los datos. En la figura 3.7 se muestra la forma en la que se conectan maestroy esclavos de la comunicación.
Figura 3.7: Topología de comunicación para cuatro esclavos
Se utiliza un protocolo ad hoc diseñado especialmente para la comunicación interna entre el maestro del
robot y los módulos de control de los motores. Se buscó un protocolo a la medida debido a las limitaciones enhardware del dispositivo utilizado por los módulos de control de los motores, el cual no cuenta con ningunainterfaz de comunicación en su circuito, haciendo necesaria su implementación por medio de software.
3.2.1.2. Bloque Comunicación (Inalámbrica)
Este módulo se encarga de permitir una comunicación bidireccional con el módulo de estrategia de acción
cooperativa, por medio de un canal inalámbrico de datos. Se basa en una configuración Maestro - Esclavo, don-de, para nuestro caso de aplicación, el único maestro de la red es el módulo de estrategia de acción cooperativa,mientras que cada uno de los robots toma el rol de esclavo de la red. La comunicación se realiza de maneraasíncrona, donde cada dispositivo debe conocer de antemano la velocidad a la cual se van a transmitir los datos,generando en cada uno la sincronía de la red sin necesidad de referenciarse a una señal de reloj común a todoslos dispositivos. La velocidad de transmisión utilizada en este caso es de 115,2Kb/s. La interfaz utilizada son losmódulos XBEE de comunicación inalámbrica, ubicados en cada uno de los robots y en el computador donde sealoja el módulo de acción cooperativa.
30
3.2.1.3. Bloque Cinemática del Robot
Este bloque se encarga de convertir las velocidades de móvil generadas por el bloque de control, en velo-cidades para cada uno de los motores del móvil. Una vez se tienen los valores de las velocidades lineales y lavelocidad angular dadas por los cálculos del control de posición, se deben convertir a velocidades que puedanser enviadas y entendidas por los módulos de control de los motores. Se tiene un bloque independiente con elfin de tener una mejor portabilidad y poder adaptar el diseño a otras topologias de robot.
3.2.1.4. Bloque Odometría
Este bloque se encarga de hacer una estimación de la posición relativa del móvil a partir de las medidasobtenidas por sus sensores de desplazamiento (encoders). Cada vez que se obtienen nuevos datos de los conteosabsolutos de pulsos por parte de los módulos de control de los motores, se realiza un cálculo de cambio deposición relativa, con respecto a la última ubicación conocida del móvil.
La salida de este bloque son 3 datos enteros representados en 2 bytes con signo cada uno, los cuales repre-sentan el cambio de posición en el eje X, en el eje Y y en la posición angular respectivamente. Este cálculode cambio de posición relativa del móvil se realiza periódicamente con un tiempo fijo, con el fin de permitiruna sincronización con los datos de la visión artificial hacía el bloque de localización y tener la información deubicación en periodos de tiempo específicos.
3.2.1.5. Bloque Control de Posición
El bloque de control de posición hace que el robot llegue al punto atractor modificando la señal de velocidadde cada una de las llantas del robot (cada llanta se mueve por medio de un motor eléctrico). Se busca que estebloque quede embarcado en el robot móvil y que este programado en el módulo maestro del robot. En la figura3.8 encuentran todos los componentes que conforman el sistema de los robots jugadores de futbol incluyendoel control, aquí se observa la gran importancia de este bloque, ya que es el que procesa la última informaciónque se traduce en movimiento de los motores del robot velocidad.
31
Figura 3.8: Esquema de control de posición dentro del robot
3.2.2. Módulo control de motores
La función principal de este módulo es la de realizar un control de velocidad en cada uno de los motores,de acuerdo con las referencias enviadas por el maestro del robot. En la figura 3.4 se muestra un diagrama debloques del módulo y la forma como se conectan entre si.
Este módulo es el encargado de todas las funciones lógicas para el manejo de los motores, entre las que sepueden enumerar:
1. Control de velocidad del motor.
2. Comunicación bidireccional con el módulo maestro del robot.
3. Lectura de la velocidad del motor.
4. Modulación y multiplexación de ancho de pulso para las variaciones de velocidad y control de giro delmotor.
32
Figura 3.9: Módulo control de motores
3.2.2.1. Bloque de Comunicación
Este bloque se encarga de permitir una comunicación bidireccional con el maestro del robot. Su función esla de recibir los datos enviados por el maestro del robot y enviar el dato acumulado de la lectura de pulsos comorespuesta. Para realizar la comunicación, se utiliza una configuración de tipo Maestro - Esclavo, dentro de unatopología de bus serial con un único maestro, donde el maestro del robot actúa como maestro de la red y cadaesclavo en la topología corresponde a un módulo de control de velocidad.
3.2.2.2. Bloque de Lectura de Pulsos
En este bloque se realiza el conteo de los pulsos generados por el encoder, con el fin de tener una medida entiempo real que permita determinar la velocidad del motor. El encoder diferencial tiene dos señales cuadradasde salida, donde el periodo y la fase entre las dos representan la magnitud y la dirección de la velocidad de girorespectivamente. La magnitud de la velocidad se toma de acuerdo a la cantidad de los pulsos obtenidos de laseñal en un periodo de tiempo establecido, en donde, cada pulso se entiende como un flanco de subida de laseñal.
3.2.2.3. Bloque de Control de velocidad
Cada valor de velocidad enviado por el maestro del robot se toma como setpoint para el control de veloci-dad. El resultado de la ejecución de los cálculos del controlador se envia al bloque PWM, que se encarga del
33
movimiento de cada motor. Este movimiento generado es leido por el encoder diferencial ubicado en uno de losextremos de cada motor, el cual envía dos señales cuadradas cuyo periodo determina la magnitud de la veloci-dad, y cuyos ángulos de fase determinan la dirección del giro. Estas señales son posteriormente interpretadas yadecuadas por el bloque de lectura de pulsos, cuya medición realimenta y cierra el lazo de este bloque.
3.2.2.4. Bloque de PWM
Este bloque se encarga de generar una señal cuadrada cuyo ciclo útil variable determina la velocidad de girodel motor, y el puerto por el cual sale determina el sentido. La entrada es un valor entero entre -100 y 100 quedetermina la magnitud y la dirección de la velocidad que debe mantener el motor, siendo -100 el máximo valorde velocidad en sentido antihorario, y 100 el máximo valor de velocidad en sentido horario.
34
Capítulo 4
Control de velocidad
En este capítulo se describe el control de velocidad implementado en cada uno de los módulos de control de
los motores. Para esto, se describe cómo se realizó la reestructuración del programa del procesador con base enlos requerimientos y necesidades del sistema.
4.1. Descripción funcionamiento procesador de los motores
Las principales características que se tuvieron en cuenta para la reestructuración del software del procesadorde los motores fueron:
1. En el módulo de comunicaciones, dado que el procesador no cuenta con hardware incluido para manejarningún protocolo estándar, fue necesario implementarlo por medio de software. Para esto se utilizó unaconfiguración Maestro - Esclavo serial, en una tipología de tipo estrella de tres líneas (reloj, datos y tierra),donde el reloj es controlado por el maestro y llega al procesador de los motores como una interrupción decambio de estado en un puerto.
2. En el módulo de Lectura de Pulsos, dado que el procesador no cuenta con hardware incluido para recibir ycontar los pulsos provenientes del encoder (típicamente un QEI, Quadrature Encoder Interface Module),fue necesario implementarlo mediante software. Para esto se ubicó una de las líneas del encoder en unaentrada con interrupción por flanco de subida, de modo que cada cuenta se realiza dentro de la interrupcióngenerada.
3. El módulo control de velocidad depende tanto del dato suministrado por la realimentación como de lareferencia. Cada vez que la referencia cambia, los cálculos se ejecutan periódicamente hasta que el controlse estabiliza, siendo el tiempo de dicho periodo determinado por la frecuencia de los datos del bloque delectura de pulsos.
Las prinicipales funciones del módulo de control de los motores, tal como se describieron en el capítulo ante-rior, consisten en un control de posición, una lectura de pulsos desde un encoder incremental, y el manejo de unprotocolo bidireccional de comunicaciones, donde cada módulo hace la función de un esclavo de la topologíaimplementada. Cada una de estas funciones depende de un evento externo al procesador del módulo, las cuales
35
son manejadas por medio de interrupciones. Debido a que el procesador no cuenta con un mecanismo que per-mita mantener una jerarquia de las interrupciones, es necesario establecer una jerarquia manual e implementarlapor medio de software.
Para esto, la tabla 4.1 muestra las interrupciones manejadas por el procesador de cada uno de los módulosde control de los motores, y la jerarquia propuesta, donde se toma como 1 a la jerarquía más importante, y deahí en adelante las menos relevantes.
Bloque Interrupción JerarquíaLectura de Pulsos Flanco de subida RA2 1
Comunicación Cambio de estado PORTA 2Control de velocidad Timer0 3
Cuadro 4.1: Jerarquía de interrupciones
Debido a que la lectura de pulsos es la forma mediante la cual el robot tiene una realimentación de suposición actual, es necesario que esta interrupción se ejecute siempre que se encuentre disponible, por lo cualocupa el primer lugar de la jerarquía.
El segundo lugar lo ocupa la interrupción correspondiente a la comunicación. Cada vez que se inicia unacomunicación, se deshabilita la interrupción producida por el control de velocidad, de modo que, mientras setiene la duración de la trama, solo se puede activar la interrupción de la lectura de pulsos. La finalidad de esto,es que, el tiempo que demora la atención de la interrupción del control no produzca un desfase con la sincroníade la comunicación y se pierda información. Para esto, se debe garantizar además que el control de velocidad
se estabilice durante el tiempo en el cual no hay comunicación.El tercer lugar lo ocupa la interrupción del control de velocidad, proveniente del Timer0 del procesador del
módulo. Mientras se ejecuta el control, se permite la entrada de cualquier interrupción de las ya mencionadas,sin embargo, la interrupción nueva solo es atendida una vez se termina la atención de la ejecución del control.
Debido a que cada bloque de manera independiente debe recibir, procesar y generar una salida de infor-mación que es vital para que todo el proceso del control de la velocidad ocurra de manera adecuada, existenuna serie de restricciones temporales para tener la mayor sincronía posible con las tareas descritas, teniendo encuenta además de las limitaciones de procesamiento y de hardware descritas en la sección 8.2.
La figura 4.1 muestra un diagrama de tiempos del funcionamiento de los bloques en el procesador de losmotores. Uno de los principales retos para el diseño fue la sincronización de los procesos que suceden de formasimultánea en los mismos periodos de tiempo, como los mostrados en el diagrama.
36
Control
Comunicación
Lectura Pulsos
T1
T2
T3
T5T4
Estable
T6
T7
Figura 4.1: Diagrama de tiempos bloque control motores
Donde:
T1→Duración de los cálculos en un ciclo de control.T2→Tiempo de establecimiento del control.T3→Duración de una trama completa de comunicación.T4→Tiempo de lectura de un pulso.T5→Tiempo durante el cual el contador de pulsos se encuentra inactivo (suponiendo máxima velocidad).T6→Tiempo en el cual se tiene una cuenta completa de pulsos para retroalimentar el control.T7→Tiempo entre comunicaciones.
La tabla 4.2 muestra los valores obtenidos a partir de las medidas temporales con un osciloscopio digital de200MHz.
Nombres TiemposT1 0,135msT2 150msT3 24msT4 0,028msT5 0,072msT6 10msT7 80ms
Cuadro 4.2: Tiempos módulo de control de motores
4.2. Cálculo de tiempos del módulo de control de motores
El cálculo de los tiempos de las señales y tiempos empleados por el procesador del módulo de control de los
motores, dependen de los tiempos que demora el procesador en atender y procesar los algoritmos dentro de lasrutinas, y de los tiempos establecidos como parámetros de diseño.
37
4.2.1. Bloque de lectura de pulsos
El bloque de lectura de pulsos realiza todo su algoritmo dentro de la interrupción por flanco de subida RA2del procesador del maestro. Cada vez que se activa dicha interrupción, se efectua el mismo algoritmo, cuyaduración total, incluyendo la entrada y la salida de la interrupción, es de 28µs.
Esta interrupción se activa cuando la señal de salida del encoder presenta un flanco de subida, la cual, ala máxima velocidad del motor se produce cada 100µs. En el cuadro 4.3 se resumen los tiempos referentes albloque de lectura de pulsos.
Acción TiempoEntrada y salida de la interrupción 20µs
Atención del algoritmo de la interrupción 8µsTiempo total de la interrupción 28µs
Tiempo máximo entre interrupciones 100µs
Cuadro 4.3: Cuadro de tiempos del bloque de lectura de pulsos
4.2.2. Bloque de control de velocidad
El bloque de control de velocidad realiza todo su algoritmo dentro de la interrupción dada por el TIMER0del procesador del maestro. Cada vez que se activa dicha interrupción, se efectua el mismo algoritmo, cuyaduración total, incluyendo la entrada y la salida de la interrupción, es de 135µs.
Esta interrupción se activa cuando el registro de conteo del TIMER0 llega a su valor máximo (255 por serun registro de 8 bits), la cual, se produce cada 10ms, que es el tiempo que demora el conteo de pulsos que sirvede referencia de realimentación del control.
Para lograr una interrupción cada 10ms, se configura el TIMER0 para que cada cuenta se realice con una fre-cuencia correspondiente a la del reloj del procesador dividido entre 4. Esto es, configurando T 0CS = 0, se tiene
que cada cuenta del timer se efectúa con un periodo igual aFosc
4= 2MHz. Ahora, configurando el bit PSA = 0,
se referencia la escala del conteo en 256, por lo cual se tiene una cuenta del timer cada256
2MHz= 128µs. De
esta forma, si la cuenta del timer inicia desde el valor P0, los 10ms requeridos para que se complete la cuenta seobtienen como:
10ms = (255−P0)×128µs (4.1)
de donde se obtiene:
P0 = 255− 10ms128µs
= 176,875 (4.2)
de modo que si se configura el registro T MR0 = 177, se tiene el conteo del timer cada 9,984ms∼ 10ms.En el cuadro 4.4 se resumen los tiempos referentes al bloque del control de velocidad.
38
Acción TiempoEntrada y salida de la interrupción 20µs
Atención del algoritmo de la interrupción 115µsTiempo total de la interrupción 135µs
Tiempo entre interrupciones 10ms
Cuadro 4.4: Cuadro de tiempos del bloque de control
4.2.3. Bloque de comunicaciones
El bloque de comunicaciones realiza su algoritmo cada vez que se presenta una interrupción de cambio deestado en PORTA del procesador. Este cambio de estado se presenta cada vez que cambia la señal de reloj de lacomunicación. El algoritmo que efectúa el bloque de comunicaciones, depende del número en que se encuentreel contador de pulsos de la señal de reloj. En este caso, dentro de la interrupción de la comunicación se realizaunicamente el cambio en el contador de pulsos, el resto del algoritmo se realiza en el bloque del programaprincipal (main) del procesador, dando lugar a que se atienda la interrupción de la lectura de pulsos cuando searequerido. De esta forma, cada vez que llega un flanco de bajada de la señal del reloj, se activa la inerrupciónque incrementa el valor del conteo de pulsos i. Una vez la interrupción ha sido atendida, dependiendo del valorde i, se llevan a cabo las instrucciones correspondientes, las cuales, aún con las interrupciones de la lectura de
pulsos deben completarse dento del tiempo que se mantenga el valor bajo de la señal del reloj. La tabla 4.5muestra los tiempos que demora el procesador en llevar a cabo las instrucciones de la comunicación, según elvalor de i.
Número de ciclo TiempoEntrada y salida de la interrupción 20µs
i = 0 9,5µsi = 1 17µs
i = 2 a i = 9 14µsi = 10 a i = 12 18µs
i = 13 26µsi = 14 a i = 21 55µsi = 22 a i = 24 47µs
i = 100 42µs
Cuadro 4.5: Tiempo de las instrucciones del bloque de comunicaciones
La interrupción del control se deshabilita en la primera llamada a la interrupción de comunicación, y sevuelve a habilitar en el último ciclo de la misma. De esta forma, el tiempo del periodo de la señal de reloj secálcula teniendo en cuenta el peor de los posibles casos, con respecto a los tiempos que puede demorar atenderlas instrucciones de un valor específico de i. Este se da en el caso en el cual se activa la interrupción del control
de velocidad, e inmediatamente se activa la interrupción de la lectura de pulsos, justo en el momento en el cualllega el flanco de bajada de la señal de reloj, activando la interrupción de la comunicación, como se observa enla figura 4.2.
En este caso, el procesador debe terminar de antender la interrupción del control, seguida por la interrupcióndel conteo de pulsos, cuya prioridad es superior a la de comunicación, y finalmente atender y ejecutar lasintrucciones correspondientes de la comunicación. Debido a que la interrupción del control se desactiva despuésdel primer ciclo de la comunicación, este caso corresponde a tener i=0.
39
T1 T2 T3 T4
T5
T6
Interrupción y cálculos del control
Interrupción y conteo de Lectura de Pulsos
Primer conteo de comunicación (instrucciones cuando i=0)
Señal de reloj de comunicación (CLK)
CLK
Interrupciones
Figura 4.2: Tiempo para periodo de comunicación
Donde:
T1 −→Tiempo para atender la interrupción de control.T2 −→Tiempo para atender la interrupción de Lectura de Pulsos.T3 −→Tiempo para atender la primera instrucción de la comunicación (i=0).T4 −→Tiempo extra de establecimiento de la señal del reloj de comunicaión.T5 −→Duración de medio ciclo de la señal de reloj.T6 −→Tiempo mínimo entre dos interrupciones consecutivas de la lectura de pulsos.Se tiene entonces que la señal del reloj debe permanecer en estado bajo por lo menos:
T1 +T2 +T3 +T4 = 135µs+14µs+9,5µs+1,5µs = 160µs (4.3)
Donde T4 se toma como un tiempo extra para el establecimiento de la señal, tiempo que se estimó en 1,5µs,de acuerdo con los datos medidos.
Los valores de tiempo correspondientes a la figura 4.2 se observan en la tabla 4.6.
Nombre TiempoT1 135µsT2 28µsT3 9,5µsT4 15,5µsT5 208µsT6 100µs
Cuadro 4.6: Tiempos del peor de los casos en el inicio de la comunicación
Por lo tanto, el valor del perido de la señal de reloj de la comunicación es igual a:
TCLK = 320µs (4.4)
40
lo que equivale a una frecuencia de:
FCLK =1
320µs= 3,125KHz (4.5)
4.2.3.1. Timer para control de errores
Con base en el tiempo calculado para el periodo de la señal de reloj, se implementó una interrupción adi-cional, encargada de reiniciar el funcionamiento normal del procesador del módulo, en el caso de tener algúnerror en la comunicación. Si el contador de pulsos se encuentra en un valor diferente a 0 y después del timempocorrespondiente a dos pulsos de reloj, no se detecta ningún cambio en la señal de CLK, se considera que hayun error en la comunicación, y se procede a la reactivación de las interrupciones, y el reinicio del contador depulsos, con el fin de poder recibir nuevas tramas de comunicación y reactivar el funcionamiento normal delprocesador.
Para esto se utilizó el TIMER1 del procesador del módulo, el cual se activa cada 2× 320µs = 640µs. SiP0 representa la primera cuenta de dicho timer de 16 bits, configurando el valor de T MR1CS = 0 para utilizarla frecuencia del procesador dividido cuatro para cada cuenta, y estableciendo el valor de T 1CKPS = 3 parareferenciar la escala de la cuenta en 8, se tiene que:
640µs = (65536−P0)×8
2MHz(4.6)
de donde:
P0 = 65536− 2MHz×640µs8
= 65376 (4.7)
De esta forma, configurando el valor de T MR1 = 65376, se tiene una interrupción cada 640µs. Esta inte-rrupción se reinicia cada vez que se tiene un flanco de bajada de la señal de reloj de la comunicación, de formaque la interrupción de TIMER1 solo se activa en el caso en el cual pasen 640µs (dos ciclos de reloj) después deque se tenga un flanco de bajada del CLK sin tener el siguiente.
4.3. Bloque de Lectura de Pulsos
Con el motor girando a su máxima velocidad con una referencia de 12 voltios, el encoder genera en sus doscanales de salida señales como las mostradas en la figura 4.3: dos señales cuadradas con un periodo de 100µs,desfasadas 90◦ una con respecto a la otra.
41
Figura 4.3: Salida del encoder, máxima velocidad @12 Voltios
Para tener una referencia de la velocidad leida que concuerde con los valores de velocidad que se le puedeaplicar al motor (-100 a 100), debe garantizarse que en el tiempo establecido para acumular la cuenta de pulsos,se tengan 100 pulsos en la señal que se genera con la máxima velocidad del motor. Es decir, teniendo en cuentaque con la máxima velocidad se tiene un flanco de subida cada 100µs, se tiene que una cuenta completa delencoder se debe realizar cada:
100×100µs = 10ms (4.8)
Con esta referencia de tiempo, se obtiene que la cantidad de pulsos leida es equivalente al valor de velocidadaplicada (es decir, si se aplica un valor de velocidad X, se va a tener un conteo de X pulsos en el intervalo detiempo obtenido), no solo con la velocidad máxima sino con cualquier valor de velocidad que se use comoreferencia; con un error cercano al 2% en el peor de los casos. De este modo, cada 10ms se tiene una medidalineal de la velocidad con un valor entre -100 y 100 indicando tanto la dirección como la magnitud de la misma,correspondiendo a la misma escala de velocidades que se tienen a la salida del control, y a la entrada del PWMque controla el motor, evitando la necesidad de utilizar factores de corrección en alguno de estos dos puntosque involucren cálculos que supongan una carga para el procesador (en este caso multiplicaciones y divisionescrean excesos de carga para el procesador utilizado) generando demoras sensibles en el procesamiento de losdatos y desfases en el funcionamiento de los bloques.
Otra de las funciones de este bloque es la de mantener un registro donde se tenga una cuenta absoluta depulsos, desde el momento en que se recibe un dato de referencia del maestro del robot, hasta que se reciba unnuevo dato. Es decir, se tiene una cuenta absoluta de pulsos que se reinicia cada 80ms y, dado que dentro de latrama solo es posible incluir datos de tamaño 1 byte (desde 0 a 255), a este conteo absoluto se le debe hacer uncorrimiento hacía la derecha de 4 bits (división entre 16) para ajustar el valor al rango permitido por el protocolode comunicaciones. Este corrimiento genera un error de máximo 15 pulsos por cada conteo absoluto, lo cual,teniendo como referencia que cada 1000 pulsos equivalen a un giro completo de una rueda, equivale a un 0,15%de un giro, es decir, un error de:
0,15100×2π×2,5cm = 0,023cm (4.9)
por cada giro de una rueda, el cual se desprecia teniendo en cuenta las dimensiones del robot y la cancha.Este conteo absoluto pasa finalmente al bloque de comunicaciones para ser enviado al maestro del robot dentrode la siguiente trama que se construya.
42
4.4. Cálculos de PWM
La frecuencia del PWM utilizada como salida hacia los motores se calculó con el objetivo de tener 100valores diferentes de velocidad en cada una de las direcciones de giro. Para el cálculo del ciclo útil de la señalde PWM en el procesador se emplea la siguiente ecuación:
CicloUtil =(CCPR1L : CCP1CON < 5 : 4 >)
4(PR2+1)(4.10)
Donde:
CCPR1L−→Registro de 8 bits donde se guarda la referencia enviada desde el control.CCP1CON < 5 : 4 >−→Dos bits utilizados en caso de necesitar una referencia de 10 bits. En este caso no
se utiliza, solo se utiliza el registro CCPR1L como referencia.PR2−→Registro de control del Timer 2. En este caso define los parámetros de salida del PWM.
Debido a que los valores del registro CCP1CON no son tenidos en cuenta, y representan los bits menossignificativos del numerador de la ecuación 4.10, se tiene que (CCPR1L : CCP1CON < 5 : 4 >) = 4×CCPR1L,de donde se obtiene:
CicloUtil =4×CCPR1L4(PR2+1)
(4.11)
CicloUtil =CCPR1LPR2+1
(4.12)
Este valor del ciclo útil define el porcentaje de la señal activa del PWM que da la magnitud de la velocidaddel motor, donde un ciclo útil igual a 1 genera la máxima velocidad del motor, y un ciclo útil igual a 0 generaque el motor permanezca detenido.
En este caso, los valores de magnitud de velocidad utilizados son números enteros entre 0 y 100, los cualesse guardan en el registro CCPR1L para variar la velocidad del motor. De esta forma, para lograr un valor deCicloUtil igual a 1 cuando el valor de CCPR1L = 100, se debe tener PR2 = 99.
Con este valor, es posible calcular el periodo de la señal de PWM obtenida, mediante la ecuación 4.13:
TPWM = [(PR2)+1]×4×TOSC× (T MR2Preescaler) (4.13)
Donde:
TPWM −→Periodo de la señal de PWM.TOSC −→Periodo del reloj del procesador.T MR2Preescaler −→Valor para escalar la cuenta del Timer2, que controla el PWM.
Utilizando un valor de T MR2Preescaler igual a 1, y dado que TOSC =1
8MHz, se tiene:
43
TPWM = 100×4× 18MHz
×1 = 50µs (4.14)
de donde la frecuencia de la señal, FPWM es igual a:
FPWM =1
TPWM= 20KHz (4.15)
4.5. Descripción del control de velocidad
Teniendo en cuenta que el motor se puede modelar como un sistema de primer orden se trata de aproximarla relación del voltaje y velocidad como un primer orden, esperando obtener errores despreciables para aplica-ción de los robots. Para controlar la velocidad se propone hacer un diseño de un controlador PI (Proporcionale Integral) debido a las limitaciones en hardware del procesador y también para obtener un lazo cerrado de conuna función de transferencia de segundo orden con la que se pueda configurar el tiempo de establecimiento y elsobre pico, en la figura 4.4 se muestra el esquema de control. Calculando el ancho de banda que se obtiene enla caracterización de la planta se diseña el control con un tiempo de establecimiento que no supere el tiempo deancho de banda y sin sobre impulso para garantizar que en el tiempo transitorio no supere la velocidad deseada.Como el control se realizó mediante un procesador se escogió un tiempo de muestreo tal que la suma de los pul-sos del encoder en máxima velocidad cuente hasta 100, después de esto se aplica la acción del control y vuelveempezar el conteo de los pulsos. Este tiempo se escoge teniendo en cuenta que el motor a máxima velocidadcuenta un pulso cada 0.1 milisegundos, con lo que quiere obtener 100 en velocidad máxima del motor paradefinirlo como el 100%. Haciendo lo anterior podemos colocar la referencia en porcentaje donde el máximo es100% (velocidad en sentido horario) y el mínimo -100% (velocidad en sentido anti horario). Para implementarel control se simula la planta con una tasa de muestreo que cumpla el 100% a máxima velocidad. La funciónde transferencia del controlador se le aplica la transformada Z por el método de backward esta función es laque finalmente se debe programar en el procesador. Debido a las limitaciones en hardware (procesador) para elcontrol se pueden usar números enteros de 2 bytes (valores enteros de -32.768 a 32.767), esto se debe a que elprocesador es de 8 bites lo que demora mucho los cálculos para números flotantes. De esta manera para opti-mizar los procesos si se obtienen números decimales primero se premultiplican las constantes proporcionales eintegrales por una constante de la forma 2n para convertirlas a un número entero significativo (si las constantesobtenidas son menores que 1), para después antes de aplicar el control dividirla por la misma constante. Todaslas ganancias se deben de escoger de la forma 2n y las divisiones de la manera 1/2n, de esta forma para multi-plicar se hacen n corrimientos binarios hacia la izquierda y para dividir se hacen n corrimientos binariuos a laderecha.
44
Figura 4.4: Diagrama de Bloques Control con Antiwindup
4.5.1. Modelamiento de los Motores
Se desea aproximar la función de transferencia del motor a un sistema lineal de primer orden y(s)u(s) =
Kτs+1
la cual tiene una respuesta en tiempo de y(t) = uK(1− e−t/τ) cuando u es una entrada paso. Sit tiene un valorigual a τ la salida queda igual a y(τ) = K(1− e−1) ' 0,6321uK, y cuando t → ∞la salida es y(t → ∞) = uK.De acuerdo a lo anterior se busca el valor de tiempo cuando la salida del motor es el 0,6321 este tiempo es τ ,también se busca el valor de establecimiento que al dividirlo sobre la entrada es K pues K = y(t→∞)
u .Los motores a velocidad máxima cuentan un pulso cada 0.1 milisegundo, para que los motores cuenten 100
pulsos a velocidad máxima hay que acumular pulsos cada 10 milisegundos. De esta manera también configu-ramos la entrada en el PWM (ver la sección del PWM) que es proporcional donde 100 significa que entra elmayor voltaje, al mismo tiempo la entrada de voltaje es proporcional a la velocidad del motor. De este modosi se ingresa x al PWM con −100 < x < 100 se espera que la salida en estado estable sea x pulsos cada 10milisegundos
Se tomaron 5 valores de K y τ aplicando una entrada de 20%, 40%, 60%, 80% y 100% de la velocidadmaxima del motor, para 3 mototres diferentes. Después de promediar los resultados se obtiene que K ' 1,07 yτ ' 71,884,104. Ver figura 4.5
45
Figura 4.5: Respuestas de dos Motores a Diferentes Entradas
La función de transferencia que representaría a la planta es:
G(s) =1,07
71,88,10−3s+1(4.16)
Experimentalmente se encontró una zona muerta en la cual el motor no se mueve aunque tiene una entradade voltaje este valor es el 7% del voltaje máximo
4.5.2. Cálculos del control de velocidad
Para controlar la velocidad del motor se diseña un control PI como se mencionó en las especificacionesfigura 4.6. La ecuación del controlador es PI:
PI(s) = Gcontrol(s) = Kp +Ki
s=
sKp +Ki
s(4.17)
Al desarrollar la ecuación del sistema retroalimentado:
Gretro(s) =Gplanta(s)Gcontrol(s)
1+Gplanta(s)Gcontrol(s)(4.18)
Como la ecuación del motor es de la forma:
Gplanta(s) =K
τs+1(4.19)
Al desarrollar el sistema retroalimentado se tiene
Gretro(s) =s(KpK
τ)+ KiK
τ
s2 + s(KpK+1τ
)+ KiKτ
(4.20)
46
De donde se obtenemos que
Ki =w2
nτ
K(4.21)
y
Kp =2ξ wnτ−1
K(4.22)
Figura 4.6: Motor Retroalimentado con el Control
Teniendo en cuenta tao y el tiempo subida al que más se demoró en responder el motor a una entrada pasose escogió como parámetros un tiempo de establecimiento al 2% de 0.2 segundos y sin sobre pico ξ = 1. Estoes:
testablecimiento = 0,2 =4
wn(4.23)
wn = 20 (4.24)
Con estos valores se tiene un Ki = 26,9 y un Kp = 1,75Para realizar la simulación se transforma la función de transferencia del motor de Laplace a transformada z
por el método Zero Order Hold y obtenemos:
G(z) =0,1388
z+0,8703(4.25)
Con el método Backward se diseña el controlador, la parte poporcional es
Prop(s) = KpE(s) (4.26)
Prop(z) = KpE(z) (4.27)
Prop(kh) = KpE(kh) (4.28)
La parte integral es:
Inte(s) =Ki
sE(s) (4.29)
47
Inte(z)' Kizhz−1
E(z) (4.30)
Inte(z)(z−1) = Inte(z)z− Inte(z) = KizhE(z) (4.31)
Inte(z)− Inte(z)z−1 = KihE(z) (4.32)
Inte(z) = KihE(z)−+Inte(z)z−1 (4.33)
Inte(kh) = KihE(kh)−+Inte((k−1)h) (4.34)
Con un tiempo de muestreo de 10 milisegundos y las constantes obtenidas anteriormente encuentra la parteproporcional del control
Prop(kh) = 1.75E(kh) (4.35)
y tambien se encuentra la aparte integral
Inte(kh) = 0,269E(kh)−+Inte((k−1)h) (4.36)
Teniendo en cuenta las restricciones del procesador que solo acepta números enteros se multiplican lasconstantes del control en la entrada por 128 o 27, esto se hace para que la parte integral tenga influencia enel control cuando son procesados, luego se divide por 128 en la salida del procesador. Se escogió 128 porquemultiplicar o dividir por 128 es hacer 7 corrimientos binarios, además con este número se hace una buenainfluencia en la parte integral. Al multiplicar el control por la constante se tiene
Prop2(kh) = 224E(kh) (4.37)
Inte2(kh) = 34,432E(kh)−+Inte((k−1)h) (4.38)
Teniendo en cuenta las limitaciones del procesador en velocidad, para realizar multiplicaciones del controlsegún la entrada del error el tiempo que se demora es del orden de milisegundos por lo que se decide a aproximarestas constantes en múltiplos de 2 así se hacen corrimientos. Estas constantes se convierten en:
Prop3(kh) = 256E(kh) (4.39)
Inte3(kh) = 32E(kh)−+Inte((k−1)h) (4.40)
En la figura4.7 se muestra la simulación en Matlab del sistema de control y motor con las constantes halladasanteriormente, con sus respectivas saturaciones, una zona muerta entre -7 y 7, un ati-windup para la parte integraly un muestreo de 10 milisegundos.
48
Figura 4.7: Diagrama de Control de Velocidad Motores
En la figura 4.8 se muestra la simulación que se obtuvo de los motores con el control discreto de velocidada las diferentes entradas paso.
4.5.3. Anti-windup
En un motor el voltaje aplicado es proporcional a la velocidad de giro, a media que las baterías se agotan elvoltaje disminuye bajando la velocidad máxima del 100%. Debido que el control diseñado tiene parte integralcuando las baterías se agoten y no puedan alcanzar el 100% la parte integral comienza a crecer desmedidamentehasta hacer que el dato se desborde (alcance el máximo tamaño permitido y comienza a contar de nuevo). Paracontrarrestar esta acción no deseada se diseña e implementa un anti-windup, esto es una realimentación de laparte integral del controlador que va de la saturación de la entrada de la planta a la parte integral. Esto hace quela parte integral del controlador sea constante cuando la entrada antes del saturador aumenta hasta cierto valor,en la figura 4.4 se muestra el diagrama de bloques del control con el bloque de antiwindup. La contante delanti-windup se escoge de tal manera que el dato no se desborde.
Figura 4.8: Simulación de Diferentes Entradas Paso a un Motor con Control
49
Para impedir que la parte integral del control crezca indefinidamente se diseñó un anti-windup de retro-cálculo y seguimiento, utilizando la ecuación ?? el valor máximo antes de la saturación antes de dividir por 128es de
PulsoAntes = PulsosSaturacion+Ki
KAnti−winduperror (4.41)
Como la constante integral es 32 para optimizar cálculos hacemos la constante del anti-windup igual a 32ver figura 4.7 con esto se tiene
PulsoAntes = 100+ error (4.42)
El error máximo puede ser 200 de -100 a 100, este error significaría que las baterías están plena carga portal razón el anti-windup no actúa para este caso. El caso más extremo es cuando las baterías están totalmentedescargadas y la referencia es 100 ó -100 el error máximo es de 100. Según la formula anterior el máximo valorantes de la saturación y de dividir por 128 es 200 lo cual es soportado ampliamente por el procesador.
En la figura 4.9 se muestra la comparación de las repuestas del control de los motores a diferentes valoresde entradas. Como se observa en la gráfica las respuestas son muy parecidas y sirven para la aplicación que setrabaja.
Figura 4.9: Comparación respuesta control motores a) paso 40. b) paso 60. c) paso 80. d) paso 100. Donde t esX 10ms.
50
Capítulo 5
Control de posición
En este capítulo se describe el control de posición embarcado en los robots. Para esto, se describen los cálcu-los de la cinemática y la odometría, que sirven como base de los diseños del mismo; y se presenta finalmenteun esquema las tareas del sistema operativo sobre el cual se programó el procesador maestro.
5.1. Cinemática del robot
Este bloque es el que se encarga de relacionar los valores obtenidos por el control directamente con la formafísica del robot, siendo sus parámetros de cálculo la cantidad de ruedas, sus ángulos de ubicación, la distanciahasta del centro del robot y el radio de la figura del robot.
Figura 5.1: Esquema geométrico robot
La figura 5.1 muestra un esquema de la geometría del robot Agentcoop actual. Consta de tres ruedas sujetasa tres motores ubicados como se muestra en el diagrama.
Cada vez que los motores del robot son activados, se obtienen las 3 fuerzas de tracción, F1, F2 y F3 observa-das en la figura, las cuales generan fuerza traslacional y torque rotacional. Cada fuerza de tracción Fi, se obtiene
51
a partir del torque del motor multiplicado por el radio de la rueda. La suma de las fuerzas depende del arregloexacto de las ruedas en el robot.
De acuerdo al análisis descrito en [21], se obtiene la siguiente ecuación:
vx
vy
ωR
=
cos(150◦) cos(270◦) cos(30◦)sen(150◦) sen(270◦) sen(30◦)
1 1 1
v1
v2
v3
(5.1)
Donde:vx −→Componente de la velocidad de móvil en el eje X.
vy −→Componente de la velocidad de móvil en el eje Y.
ω −→Velocidad ángular del móvil.vi −→Velocidad del motor i.
Partiendo de esta relación, si llamamos vM =(
vx vy ω
)a las componentes de la velocidad de Móvil,
vm =(
v1 v2 v3
)a las velocidades de motor, y Mv a la matriz de acoplamiento de velocidades descrita,
la ecuación 5.1 describe la relación vM = Mvvm, de donde, al ser Mv una matriz invertible, se tiene que vm =
M−1v vM .
De esta forma se obtienen dos expresiones que relacionan las velocidades de móvil con las velocidades decada uno de los motores y viceversa. vx
vy
ωR
=
−0,86603 0 0,866030,5 −1 0,51 1 1
v1
v2
v3
(5.2)
v1
v2
v3
=
−0,57735 0,33333 0,33333−0,66667 0 0,333330,57735 0,33333 0,33333
vx
vy
ωR
(5.3)
En el robot se cuentan los pulsos de los encorder, que representan una rotación de la rueda y equivalen a unmovimiento lineal del robot. Estos pulsos son contados cada 10 milisegundos. Dado que la velocidad permanececonstante en este instante de tiempo, se puede estimar la velocidad en dicho intervalo. La distancia lineal en unarueda es 2πr. La medida del radio de la rueda es de 2.5 cm. Ya que vueltas = pulsos
1000 , y teniendo en cuenta losparámetros anteriores, se calcula el factor que convierte de velocidad a pulsos:
v =2·π ·2,5
(1000)(0,01)P (5.4)
v =5π
10P (5.5)
Donde P es el numero de pulsos, y v se mide en cm/s.Combinando la ecuación 5.2 con 5.5 y teniendo en cuenta que el radio del robot es aproximadamente 8cm,
si se define Pi como la cantidad de pulsos leidos de la i-ésima rueda, se tiene la siguiente ecuación:
52
vx
vy
8ω
=5π
10
−0,86603 0 0,866030,5 −1 0,51 1 1
P1
P2
P3
(5.6)
vx
vy
ω
=
−1,36 0 1,360,785 −1,57 0,7850,2 0,2 0,2
P1
P2
P3
(5.7)
De igual modo para encontrar el número de pulsos que se necesitan con cierta velocidad se encuentra lamatriz inversa P1
P2
P3
=
−0,3675 0,2122 1,69760 −0,4244 1,6976
−0,3675 0,2122 1,6976
vx
vy
ω
(5.8)
Si el ω de entrada esta en grados en la ecuacion anterior se multiplica la ultima columna por 2π
160 : P1
P2
P3
=
−0,3675 0,2122 0,02960 −0,4244 0,0296
−0,3675 0,2122 0,0296
vx
vy
ω
(5.9)
Paran verificar la ecuación anterior experimentalmente se encontraron velocidades máximas en x, y y θ . Losresultados obtenidos fueron vmaxx = 181,81cm/s, vmaxy = 153,84cm/s y vmaxθ = 1136,84cm/s.
Esta disminución en las velocidades máximas se debe principalmente a variables de fricción e inercia que nose tuvieron en cuenta; además del desgaste de la carga de las baterías. Finalmente con los valores encontradosexperimentalmente de volocidades máximas se obtiene la matiz que convierte las velocidades a un valor deporcentaje que sería el parámetro de entrada al motor como: P1
P2
P3
=
−0,55555 0,325 0,0880 −0,650 0,088
−0,55555 0,325 0,088
vx
vy
ω
(5.10)
5.2. Odometría
La velocidad del robot viene dada por los cálculos del bloque de control del módulo maestro del robot,
y se hace efectiva una vez los motores han recibido el dato correspondiente a sus velocidades, provenientede una trama de comunicación entre el módulo maestro del robot, y los módulos de control de los motores.
Esta comunicación se realiza periódicamente, con un tiempo específico entre cada trama. En este caso, cada∆t = 80ms se produce la comunicación que permite el cambio de velocidad del móvil y la lectura de los datosde los encoders, para poder realizar el control de posición correspondiente.
De esta forma, cada ∆t segundos, la velocidad del móvil se puede considerar como una constante, de modoque la distancia recorrida por el robot durante este intervalo de tiempo viene dada por x = v×∆t. Ahora, unavelocidad constante en el robot se produce debido a una velocidad constante de cada uno de sus motores. Por lotanto, dentro del intervalo de tiempo ∆t, de la ecuación 5.2 se obtiene:
53
x
y
Rθ
=
−0,86603 0 0,866030,5 −1 0,51 1 1
m1
m2
m3
(5.11)
Donde:x−→Distancia recorrida por el móvil en el eje X.
y−→Distancia recorrida por el móvil en el eje Y.
R−→Radio del robot.θ −→Ángulo total de giro del robot.mi −→Distancia recorrida por la rueda del motor i.
La expresión Rθ representa la distancia total de giro efectuado por el robot.. Esta distancia viene dada por elángulo total de giro del robot, multiplicado por el radio del mismo. Cada rueda del robot recorre una distanciaigual a 2πr por cada vuelta completa dada, donde r representa el radio de cada una de las ruedas, 2,5cm en estecaso.
Figura 5.2: Radio de la rueda
La cantidad de vueltas dada por cada una de las ruedas en el tiempo ∆t, viene dada por el valor del conteoabsoluto obtenido del bloque de lectura de pulsos de cada módulo, donde cada 1000 pulsos equivalen a unavuelta efectiva de la rueda correspondiente. De esta forma, la cantidad de vueltas es igual a la cantidad depulsos leidos divididos entre 1000. Por lo tanto, si x es la distancia total recorrida, en centímetros, por cadarueda en el tiempo ∆t, y P es la cantidad de pulsos leida en el mismo intervalo de tiempo, se tiene que:
x = 2πrP
1000= 5π
P1000
(5.12)
Entonces, de la ecuación 5.2 se puede obtener: x
y
Rθ
=5π
1000
−0,86603 0 0,866030,5 −1 0,51 1 1
P1
P2
P3
(5.13)
Donde:Pi −→Cantidad de pulsos leidos de la rueda i.
54
x =13,60349(P3−P1)
1000(5.14)
y =15,70963
(P1
2−P2 +
P3
2
)1000
(5.15)
Rθ =15,70963(P1 +P2 +P3)
1000(5.16)
Por ultimo se tiene la ecuación que relaciona distancia y ángulo con pulsos: x
y
θ
=
−0,01306 0 0,013060,00785 −0,01509 0,00785
0,0019634 0,0019634 0,0019634
P1
P2
P3
(5.17)
Donde x y y están en centímetros, y θ en radianes.Ahora, debido a que el dato obtenido de la lectura de pulsos de cada módulo es igual a la cantidad de
pulsos del conteo absoluto dividido entre 16, y teniendo en cuenta que el radio del robot es de 8cm, se obtienenfinalmente las ecuaciones: x
y
Rθ
=5π
1000×16×
−0,86603 0 0,866030,5 −1 0,51 1 1
L1
L2
L3
(5.18)
x = 0,21765× (L3−L1)
y = 0,12566× (L1−2L2 +L3)
θ = 0,03141× (L1 +L2 +L3)
(5.19)
x
y
θ
=
−0,21765 0 0,217650,12566 −0,25112 0,125660,03141 0,03141 0,03141
L1
L2
L3
(5.20)
Donde:Li −→Conteo de pulsos enviado desde el módulo de control i, representa el total de pulsos obtenidos del
encoder, divido por 16.
Como el movimiento circular se mide en radianes y es cíclico (valores entre−π y π), es posible utilizar unavariable del tipo byte con signo (valores desde -127 hasta 128 positivos), que se comporte de la misma forma.En este caso, si 128 corresponde a π en radianes, esta transformación hace que cuando se le sume una unidad a128, se retorna a -127; y de la misma forma cuando disminuye una unidad y se encuentra en -127, se retorna alvalor 128. Esto permite un comportamiento cíclico como se desea. Para hacer esta conversión se multiplica losradianes por 128/π . Y se obtiene:
55
θ = 0,03141× (L1 +L2 +L3)×128/π (5.21)
x
y
θ
=
−0,21765 0 0,217650,12566 −0,25112 0,125661,25663 1,25663 1,25663
L1
L2
L3
(5.22)
Donde θ corresponde a un valor entre -127 y 128.Finalmente el ángulo después de pasar por el proceso cíclico se escala por 180
128 = 1,40625 para convertirloen grados.
θ∗ = 1.25663(L1 +L2 +L3)×1.40625 (5.23)
θ ∗→Es el ángulo convertido en grados.Para verificar la ecuación anterior se realizaron pruebas contando cuantos pulsos se obtienen cuando el robot
anda un metro en x y y, para teta se contaron los pulsos sontaos cuando el robot da un vuelta. Se encontró quecuando el robot se mueve con velocidades cercanas al 100% de la velocidad máxima las ruedas patinan y setienen resultados no linelaes con mucho error al aplicar la ecuación 5.22, en cambio cuando el robot me mueve avelocidades moderadas las ruedas no patinan y los pulsos son proporciones al desplazamiento. Haciendo variaspruebas a velocidades moderadas se obtuvo: x
y
θ
=
−0,11739 0 0,117390,110 −0,220 0,1100,601 0,601 0,601
L1
L2
L3
(5.24)
Se encuentra que en x y y varían un poco por errores sistemático en las mediciones de robots y θ varíadrásticamente porque las llantas no se adhieren a la superficie de la cancha y tienen histéresis mecánica nocontemplada.
5.3. Cálculos control de posición
Para implementar el control de posición de un robot holonómico según se consultó en varios artículos como[21] y [10] y se observó que utilizan controladores PID con resultados significativos. Por esta razón se decidióimplementar un controlador PID para cada eje de robot x, y y θ . Para realizar sintonización de los controladoresPID se simula todo el sistema de los robots en Matlab y con una malla de puntos se minimiza la integral delerror teniendo como entrada una señal paso. Antes de aplicar el al algoritmo de control hay que tener en cuentaque el robot aunque se representa como un punto x, y y θ tiene ciertas dimensiones que se pueden aproximara un rectángulo. En ocasiones el atractor puede llegar a ser el balón y el robot no puede están encima delbalón. Teniendo en cuenta estas dimensiones y que la información es digital se crea la necesidad de tener unazona muerta para cada una de las coordenadas en las que el robot no se mueva. También cuando las distanciasson muy grandes se saturan los controladores haciendo que los valores crezcan indefinidamente eso altera larespuesta del control cuando sale de la zona de saturación. Para arreglar este problema se integra un saturadorque solo aplica el control cuando está en la zona valida (ver figura 5.3) saturador o zona muerta cuando no estáen zona valida de control PID y la distancian al atractor es mayor a la permitida se satura la salida (máxima
56
velocidad en x, y y θ ) y si la distancia al atractor es menor a la permitida deja a velocidad se vuelve cero, esteproceso se hace para cada una de las coordenadas. Es siguiente paso después de obtener las velocidades x, y
y θ es convertir estas velocidades a voltaje de motor en cada coordenada para después superponerlas (sumarlos voltajes que corresponden a cada motor especifico). Para garantizar que todos los motores estén en el rangode voltaje y el robot se mueva hacia la dirección correcta antes de enviar el voltaje a los motores los valoresde voltaje de los tres motores pasan por un saturador que normaliza proporcionalmente todos estos valores devoltaje, esto es si la salida de mayor valor sobrepasa el límite de salida (más del 100% de voltaje de motores).Cuando la mayor salida supera el límite esta se cambia a la máxima permitida según las especificaciones delmotor, las otras dos salidas se disminuyen proporcionalmente se disminuyó la mayor salida (se multiplican unfactor que es igual al mayor voltaje permitido dividido entre la mayor salida de voltaje). Luego estas salidas devoltaje son aplicadas a los motores para después por medio de la odmetría y la cámara encontrar la posición derobot. Por último con el punto atractor y la posición se calcula el error para volver a comenzar el ciclo anterior.Todos estos pasos se pueden observar en la figura 5.3. Con este control se espera minimizar la integral del errorde posición lo que se traduce en mayor velocidad del robot hacia el atractor.
Figura 5.3: Diagrama de Control
Para que el robot llegue al punto atractor se diseñó un control PID independiente para cada eje. La figuramuestra el control que se diseñó, simuló en Matlab e implementó en el procesador master (embarcado en elrobot) para la corregir el error de posición.
57
Figura 5.4: Diagrama de bloques del control de posición del robot.
En el diseño del control no se tuvieron en cuenta los errores de edometría y la corrección de la cámara. Enla simulación se asume que las llantas no patinan, no existe histéresis mecánica entre el motor y la rueda y losencoder no acumulan error. Ahora se describe cada sección en la implementación del control.
5.3.1. Cambio del eje de referencia
La cámara tiene un eje de referencia fijo (x,y) no varía respecto la cancha (referencia global), en cambio eleje de referencia del robot siempre es el mismo para el robot pero si varia con respecto a la cancha y cámarao referencia global (ver figura 5.5). El ángulo o tercera coordenada del atractor y robot está medido respectola referencia global o de la cámara. Este ángulo ayuda a convertir de la referencia global a la referencia fija oviceversa.
Para calcular los valores de la salida de control de posición se realiza una estima de la posición entre laodometría y las coordenadas de la cámara. La cámara tiene coodenadas globales o de la cancha, mientas laodometría se realiza mediante la integral de x y y con referencia al robot por lo que se tiene que convertir delas coordenadas del robot (fijas) a las coordenadas de la cámara (globales) para luego realizar la estima de laposición con los datos de la cámara. Para realizar el cambio de coordenadas se observa que el robot está rotadocon respecto a la cancha, entonces por medio de las funciones trigonométricas, como se muestra en la figura5.6, se realiza una proyección ortogonal al nuevo eje de referencia (x′,y′) y se encuentra la matriz de rotaciónteniendo en cuenta que el ángulo positivo es en contra de las manecillas del reloj.
58
Figura 5.5: Ejes de referencia
x′
y′
θ ′
=
cos(θ) sen(θ) 0−sen(θ) cos(θ) 0
0 0 1
x
y
θ
(5.25)
Cuando se quiere cambiar del eje de referencia o de robot al eje global de la cancha se gira en sentidocontrario a las manecillas del reloj pero el ángulo se mantiene positivo; entonces se utiliza la siguiente matrizde rotación: x
y
θ
=
cos(θ) −sen(θ) 0sen(θ) cos(θ) 0
0 0 1
x´
y′
θ ′
(5.26)
Para encontrar senos y cosenos en la implementación se utilizaron tablas con valores guardados desde laprogramación por software. Previo a aplicar la matriz de rotación se u utiliza la ecuación 5.22 para convertir depulsos de motores a poción (x′,y′). Esto es lo que se muestra antes de la integral en la figura 5.4.
59
Figura 5.6: Rotación eje de coordenadas
Después de tener la posición estimada se resta a la posición deseada obteniendo el error, pero como se estátrabajando con el sistema de control embarcado y tiene que mover al robot respecto a su eje de referencia antescalcular el control de posición, se tiene que rotar el error por medio de la ecuación 5.25. En la simulación grafica5.4 se puede observar el bloque ‘X rotad y Y rotad’ los cuales hacen referencia a esta sección.
5.3.2. Descripción Control de posición
Para diseñar el control se tuvo en cuenta que el movimiento en cada coordenadas (x,y,θ) según la ecuaciön5.7son independientes, aunque experimentalmente para alguna coordenada pueda ser que no sea tan cierto debido aque el robot patina, retardos y aproximaciones que se utilizaron. Para cada coordenada se diseña un control PIDóptimo que se obtiene dejando el error de dos coordenadas en cero y creando una malla de puntos para diferen-tes constantes proporcional, integral y derivativa, ingresando como entrada una señal paso con una magnitud deuno. Las constantes óptimas para x fueron
kp = 6,48ki = 0kd = 0,23
Las constantes óptimas para y fueron
kp = 6,72ki = 0,01kd = 0,33
Las constantes óptimas para θ fueron
60
kp = 7,3ki = 0,75kd = 0,14
Aunque no se tiene completamente la certeza de que las constantes anteriores sean un mínimo global dela integral del error, los valores encontrados en las simulaciones que se realizaron con la malla de puntosconvergieron a estas constantes, también se realizaron experimentos con valores de error diferentes de ceroarrojando los mismos resultados. En la gráfica 5.7 se muestran las simulaciones ingresando una señal paso conmagnitud de uno a cada una de las coordenadas.
Figura 5.7: Respuesta control optimo entrada paso a) grafica x-y. b) grafica t-x. c) grafica t-y. d) grafica t-θ .
5.3.3. Saturador de velocidad de motores
Después de realizar el cálculo de las velocidades de motores en el control, los motores podrían llegar asaturarse. Si esto llegara a suceder la dirección calculada del robot se ve afectada, en el caso en que se saturenlos tres motores el robot solo giraría y no tendría cambio de dirección en x o y. Esto es fácil de deducir si seobserva la matriz en la ecuación 5.2 reemplazando los valores de velocidad por 100.
61
Para que el robot no cambie su dirección independientemente de su velocidades se puede saturar x, y y θ
para que al superponerlos no se saturen los motores. Esta propuesta estaba implementada anteriormente en losrobots, pero esto hacía que los robots fueran lentos cuando solamente hacían recorrido en una solo eje del robot.
Para solucionar el problema anterior se diseñó un saturador que se activa cuando alguna de la velocidadesde los motores superan el 100%, si esto llegara a suceder se disminuye proporcionalmente todas las velocidadesde los motores de tal manera que la velocidad máxima quede en 100%
Figura 5.8: Diagrama de la saturación de velocidades de motor
Al disminuir proporcionalmente la velocidad en todos los motores el robot no cambia la dirección hacia lacual se mueve, además cuando el robot se mueve en un solo eje su velocidad también es grande. En la figura 5.9se puede observar la simulación del saturador proporcional.
Figura 5.9: Diagrama de la saturación velocidades de motor en matlab
62
5.3.4. Control de posición implementado
Figura 5.10: Control de pocicon Implementado
En la figura 5.10 se observa el control que se utilizó finamente. El control actúa solamente en un rangode error para cada coordenada. En la programación real del robot existen tres zonas para cada coordenada. Laprimera zona es saturación: aquí el robot coloca el máximo de velocidad vsat . La segunda zona es control: dondeactual el control y comienza a una distancia dcon. La tercera e última zona es muerta: no hay alguna acción sobreel robot a una distancia llamada zm.
Cuando se realizaron pruebas se encontró que como en la ecuación 5.22 se aproximó que mientras que elmóvil se mueve en x y/o y al mismo tiempo que en θ , x y/o y son independientes de la velocidad en θ . Esto escierto si la velocidad de θ es cero o cercana a cero, cuando se tienen altas velocidades en θ al mismo tiempoque en x y/o y el error en odometría aumenta considerablemente, haciendo que el robot pierda su trayectoriay se demore mucho más tiempo en llegar a su atractor. Además cuando se tienen velocidades altas en x y y
combinada con una velocidad baja en θ el cambio es despreciable en ángulo. Teniendo en cuenta lo anterior seoptó por anular la acción de control en θ cuando la distancia al atractor fuera menor que 30 cm, ya que en estadistancia no ha llegado completamente al atractor y sus velocidades x y y son relativamente bajas. Este activadordel control teta se muestra en la figura 5.10.
En la zona de saturación para x se escogióvsatx = 80% de la velocidad máxima en x, esto se tuvo en cuentaporque debería de ser la mínima tensión entregada por la batería. Para y se escogió vsaty = 70% de la velocidadmáxima en y. Este valor se obtuvo probando que el robot no perdiera su dirección porque patinaran las llantas.Para θ se escogió vsatθ = 17% de la velocidad este valor se escogió probando que cuando el control se activarano afectara la llegada en x y y.
En la zona de control se utilizaron los controladores PID mencionados anteriormente, pero se escaló lsaseñal de control por unas constantes k1 para x, k2 para y y k3 para θ (figura 5.10), y así tratar de hacer coincidirlas velocidades en las fronteras. Las distancias dcon se encontraron de manera que cuando el robot llegara a lazona muerta no pasara la mitad de la distancia de la zm. Los valores encontrados de distancia son, dconx= 140cm,dcony = 125cm y dconteta = 90° y de las contantes son: k1 = 0,18, k2 = 0,165 y k3 = 0,35.
La zona muerta de los robots se escogió por las dimensiones del robot y la pelota. Como el robot se puederepresentar como un cÍrculo en (x,y) zmx = zm, y = 20cm. Para θ como el balón es una pelota de golf, según
63
sus dimensiones zmθ = 5°.En la figura 5.11 se muestra la respuesta del control simulado del control implementado.
Figura 5.11: Respuesta control Implementado a) grafica x-y. b) grafica t-x. c) grafica t-y. d) grafica t-teta. Dondet es tiempo.
5.3.4.1. Pruebas y resultados
Para la validación del control implementado, se realizaron cuatro pruebas de seguimiento de trayectoria, conel fin de analizar la respuesta a diferentes escenarios que se puedan presentar.
Prueba de desplazamiento en X Los parámetros de esta prueba fueron:
X Y ThetaPosición incial 0 0 0
Posición deseada 115 0 0
Cuadro 5.1: Parámetros para prueba de desplazamiento X
Los resultados obtenidos, tanto de la simulación como de los datos reales se observan en la figura 5.12.
64
Figura 5.12: Resultados en X - Prueba desplazamiento en X
Prueba de desplazamiento en Y Los parámetros de esta prueba fueron:
X Y ThetaPosición incial 0 0 0
Posición deseada 0 100 0
Cuadro 5.2: Parámetros para prueba de desplazamiento Y
Los resultados obtenidos, tanto de la simulación como de los datos reales se observan en la figura 5.13.
Figura 5.13: Resultados en Y - Prueba desplazamiento en Y
65
Prueba de desplazamiento en Theta Los parámetros de esta prueba fueron:
X Y ThetaPosición incial 0 0 0
Posición deseada 0 0 170
Cuadro 5.3: Parámetros para prueba de desplazamiento en Theta
Los resultados obtenidos, tanto de la simulación como de los datos reales se observan en la figura 5.14.
Figura 5.14: Resultados en X - Prueba desplazamiento en Theta
Prueba de desplazamiento distancias cortas Los parámetros de esta prueba fueron:
X Y ThetaPosición incial 0 0 0
Posición deseada 30 30 90
Cuadro 5.4: Parámetros para prueba de desplazamiento en distancias cortas
Los resultados obtenidos, tanto de la simulación como de los datos reales se observan en la figura 5.15.
66
Figura 5.15: Resultados - Prueba desplazamiento distancias cortas
Prueba de desplazamiento distancias largas Los parámetros de esta prueba fueron:
X Y ThetaPosición incial 0 0 0
Posición deseada 135 105 90
Cuadro 5.5: Parámetros para prueba de desplazamiento en distancias largas
Los resultados obtenidos, tanto de la simulación como de los datos reales se observan en la figura 5.16.
67
Figura 5.16: Resultados - Prueba desplazamiento distancias largas
Los resultados muestran la respuesta real es muy cercana a la respuesta obtenida por la sumulación. Sepresentan errores muy bajos, cercanos al 4% en promedio, con casos aislados de errores mayores de trayectoría,que son debidos a situaciones de la parte mecánica que generan comportamentos no deseados. Dos factores deerror fueron identificados: El primero, un error que se presenta en el arranque brusco del robot, que generadeslizamientos de las ruedas sobre la superficie en la cual se encuentra, provocando errores de odometría ydesvios en la trayectoria programa. El segundo factor identificado es la carga de las baterías, que impide alcanzarciertos valores de velocidad en los motores, lo cual genera también desvíos e imprecisiones en las trayectoriasestudiadas.
Se realizaron también dos pruebas de UBMark para la odometría, cuyos resultados se muestran a continua-ción:
68
Figura 5.17: Pruebas de UBMark para odometría
En la figura 5.17 se observa la trayectoría seguida por el robot, ante la prueba de recorrer una forma cuadradaexacta. Se presentan errores en cada uno de los ejes, que son debidos a la presencia de zonas muertas en elcontrol de posición. El resultado de estas pruebas confirma la presencia de errores externos ajenos al cálculo delcontrol propuesto.
5.4. Descripción diagrama de tareas procesador maestro
La figura 5.18 muestra un diagrama de la tareas del procesador maestro, las colas para la comunicación delas mismas, y las interrupciones implementadas.
La tabla 5.6 muestra un resumen de las prioridades de cada una de las tareas, siendo 1 la prioridad más baja.
Tarea PrioridadCommSlaves 3
ContolPosicion 3Localización 2
PhysicalReceiver 1PhysicalSender 1
Cuadro 5.6: Tabla de prioridades de las tareas
69
Figura 5.18: Diagrama de tareas módulo maestro del robot
La tabla 5.7 muestra el tamaño de las colas utilizadas para comunicar las tareas del procesador del maestrodel robot, así como el tipo de datos utilizado en cada una.
Cola Tamaño Tipo de DatoQueueReceiver TstDatos_Xbee
8 datos unsigned charQueueCamara TstPosicion_XYT
3 datos intQueueAtractor TstPosicion_XYT
3 datos intQueueEncoder123 TstDatos_Motores
3 datos signed charQueueLocalizacion TstPosicion_XYT
3 datos intQueueVelocidadesXYW TstVelocidades_XYW
3 datos signed char
Cuadro 5.7: Tabla de colas del procesador maestro
70
5.4.1. Interrupción UART
Estado DescripciónSe habilita Desde el inicio del programa y cada vez que termina una comunicación con los esclavos
Sedesabilita
Cada vez que inicia una comunicación con los esclavos
Se activa Cada vez que llega un byte de la comunicación inalámbicaEntradas Datos provenientes de la comunicación inalámbrica→ 1 dato de tipo unsigned charSalidas Un dato a la cola QueueReceiver→ 1 dato de tipo TstDatos_Xbee (8 datos tipo unsigned char)
Cuadro 5.8: Tabla detalles interrupción UART
La interrucipción UART se activa cada vez que llega un byte proveniente de la comunicación inalámbrica,enviado por el módulo de estrategia de acción cooperativa. Su función es la de recibir cada uno de los bytesenviados por la comunicación y guardarlos en un buffer circular, el cual acumula los datos recibidos, hastael momento en el que detecta una trama válida según el protocolo de comunicaciones (ver 3.2.2.1). En esemomento, envía dichos datos a la cola QueueReceiver.
5.4.2. Interrupción CommSlaves
Estado DescripciónSe habilita Por la tarea CommSlaves, cada vez que se tienen velocidades de motores listas para enviar (cada
80ms después del control)Se
desabilitaCuando termina una comunicación con los esclavos
Se activa Por la acción del TIMER2 del procesador maestro (cada 160µs)Entradas Datos provenientes de la tarea CommSlaves→ 1 dato tipo TstDatos_Motores (3 datos de tipo
signed char)Salidas Datos a la cola QueueEncoder123→1 dato tipo TstDatos_Motores (3 datos tipo signed char)
Cuadro 5.9: Tabla detalles interrupción CommSlaves
La interrupción CommSlaves se habilita en la tarea CommSlaves, una vez se tienen velocidades de motoreslistas para comunicar. Estas velocidades se calculan a partir de las velocidades de móvil obtenidas por el controlde posición, de modo que, la comunicación con los esclavos, que comienza con la habilitación de la interrupción,se ejecuta cada 80ms.
Una vez se habilita la interrupción,en este caso la del TIMER2 del procesador maestro, cada 160µs se activanuevamente, hasta que termine la comunicación con los esclavos. La interrupción tiene la función del envío delreloj de la comunicación hacia cada uno de los esclavos, y el envío de los datos correspondiente en cada unode los ciclos de reloj (ver sección 6.2.1). Dado que la comunicación es bidireccional, al tiempo en que se estanenviando las velocidades de motor a cada uno de los esclavos, se estan recibiendo los datos de las lecturasacumuladas de los encoders. Dichos datos se escriben en una variable de tipo TstDatos_Motores y se envía a lacola QueueEncoder123.
71
5.4.3. Tarea Control de Posición
Estado DescripciónSe activa Cada 80ms por medio de una función vTaskDelayUntilEntradas
Datos de la cola QueueAtractor→ 1 dato tipo TstPosicion_XYT (3 datos tipo int)
Datos de la cola QueueLocalizacion→1 dato tipo TstPosicionXYT (3 datos tipo int)
Salidas Datos a la cola QueueVelocidadesXYW→1 dato tipo TstVelocidad_XYW (3 datos tipo int)
Cuadro 5.10: Tabla detalles tarea control de posición
La tarea Control de Posición se activa cada 80ms. Su función es la de realizar los cálculos descritos delcontrol de posición, con base en la localización del robot, proveniente del dato de la cola QueueLocalizacion yel dato del atractor, proveniente del dato de la cola QueueAtractor.
Cada vez que la tarea de control se ejecuta, revisa si existe una actualización de los valores de referenciadados por las dos colas de entrada, y realiza los cálculos correspondientes. En caso de no tener actualizacionesen dichos datos, sigue calculando las velocidades de móvil con los datos de referencia anteriores, de modo quesiempre se tiene un valor escrito en la cola QueueVelocidadesXYW correspondiente a la velocidad de móvil,cada vez que la tarea se ejecuta.
5.4.4. Tarea Comunicación con esclavos
Estado DescripciónSe activa Con la cola QueueVelocidadesXYWEntradas Datos de la cola QueueVelocidadesXYW→ 1 dato tipo TstVelocidad_XYW (3 datos tipo int)Salidas Velocidades de motores→1 dato tipo TstDatos_Motores
Cuadro 5.11: Tabla detalles tarea comunicacion con esclavos
La tarea CommSlaves se activa cada vez que hay un dato de velocidad de móvil en la cola QueueVeloci-
dadesXYW, proveniente de los cálculos del control de posición. Su función es la de realizar la conversión develocidades de móvil, a velocidades de motores para ser enviadas a la interrupción de CommSlaves. Se encargade, una vez realizados los cálculos de velocidades de motor, activar la interrupción que permite la comuni-cación entre el módulo maestro del robot y los módulos de control de los motores. Esto con el fin de evitarque la interrupción se active cuando no hay una comunicación pendiente, y evitar un gasto de procesamientoinnecesario.
72
5.4.5. Tarea Control Localización
Estado DescripciónSe activa Con la cola QueueEncoder123Entradas
Datos de la cola QueueEncoder123→ 1 dato tipo TstDatos_Motores (3 datos tipo signedchar)
Datos de la cola QueueCamara→1 dato tipo TstPosicion_XYT (3 datos tipo int)
Salidas Dato a la cola QueueLocalizacion→1 dato tipo TstPosicion_XYT (3 datos tipo int)
Cuadro 5.12: Tabla detalles tarea localizacion
La tarea Localizacion se activa cada vez que hay un dato en la cola QueueEncoder123. Su función es la deproporcionar un dato que represente la localización actual del móvil, para que sirva de referencia a la tarea delcontrol de posición.
Cada vez que recibe un dato de la cola QueueEncoder123, revisa la cola QueueCamara, de modo que, sirecibe un dato de dicha cola, lo envía a la cola QueueLocalizacion como el dato de posición actual del robot. Encaso de no recibir dato de cámara, realiza los cálculos de odometría (ver sección 3.2.1.4), y envía el resultadoacumulado a la cola QueueLocalizacion.
5.4.6. Tarea Recepción de datos Xbee
Estado DescripciónSe activa Con la cola QueueReceiver
Entradas
Datos de la cola QueueReceiver→1 dato tipo TstDatos_Xbee (8 datos tipo unsigned char)
Dato a la cola QueueAtractor→1 dato tipo TstPosicion_XYT (3 datos tipo int)
Salidas Dato a la cola QueueCamara→1 dato tipo TstPosicion_XYT (3 datos tipo int)
Cuadro 5.13: Tabla detalles tarea localizacion
La tarea PhysicalReceiver se activa cada que hay un dato en la cola Receiver. Su función es la de reci-bir una trama de comunicación válida enviada por el módulo de estrategia de acción cooperativa, y según elencabezado, convertir los datos recibidos en datos válidos de posición, para enviar a la cola QueueAtractor oQueueCamara, según sea el caso.
5.4.7. Tarea Envio de datos Xbee
Estado DescripciónSe activa Con la cola QueueSenderEntradas Datos de la cola QueueSender→1 dato tipo charSalidas
Cuadro 5.14: Tabla detalles tarea envío de datos
73
La tarea PhysicalSender se activa cuando hay datos en la cola QueueSender. Su función es la de enviar losdatos que haya en la cola, por medio de la comunicación inalámbrica, hacía el módulo de estrategia de acción
cooperativa. Envía un único byte cada vez y es utilizada en el proyecto para tareas de depuración.
74
Capítulo 6
Comunicación
6.1. Comunicación (Backplane)
Al iniciar el programa, el bloque de comunicaciones del maestro y los esclavos inician una rutina con el finde detectar cuantos esclavos se encuentran conectados en el robot. El maestro inicia la comunicación enviandouna trama con el código Reqtoslave a cada uno de los esclavos, a lo cual, dentro de la misma trama, cada esclavoactivo en el robot debe responder con el código Acktomaster. Si no se recibe la respuesta de algún esclavo enparticular, se considera que a partir de ese número no se encuentra activo ningún otro esclavo. Una vez termina laprimera trama, el maestro realiza el conteo de la cantidad total de esclavos activos, y envía el código Numslaves
hacía todos los esclavos presentes. Una vez cada módulo de control de los motores recibe el dato enviado desdeel maestro, guarda la información de la cantidad total de esclavos activos, y responde al maestro con un nuevocódigo Acktomaster, como se ve en la figura 6.1.
Reqtoslave
Acktomaster
Numslaves
Acktomaster
Maestro
Esclavo
Figura 6.1: Número de esclavos
La longitud de cada una de las dos tramas enviadas depende del parámetro global Maxslaves, el cual esun indicador del número máximo de esclavos que pueden estar conectados a uno de los robots. En este casoMaxslaves es igual a 5, por lo que cada trama inicial equivale al envío de datos hacia 5 esclavos en total. Elenvío de los datos y las respuesta responden al formato de trama en el cual se tiene un dato hacia un esclavo yla respuesta del mismo, intercalados para cada esclavo, como se observa en la figura:
75
Velmotor 1
Encoder 1
Velmotor 2
Encoder 2
Maestro
Esclavo
Velmotor n
Encoder n
Figura 6.2: Trama de datos
A partir de este punto, tanto el módulo maestro del robot, como cada uno de los módulos de control de los
motores conocen el número de esclavos activos, por lo cual, a partir de la tercera trama, la longitud de cadatrama de comunicación es del tamaño correspondiente al número de esclavos activos, lo cual, para el caso de losrobots diseñados para la liga SSL, corresponde a 3 esclavos y tramas de longitud de tamaño 25×3 = 75 bits.
6.2. Descripción del protocolo
El maestro del robot es el encargado de controlar en que momento comienza una comunicación. Una veziniciada, el mismo se encarga de generar la señal de reloj cuya función es la de sincronizar la lectura y escriturade cada bit de datos, y permitir un conteo de ciclos que marque el final de la comunicación.
El protocolo se basa en una trama con la forma mostrada en la figura 6.3, donde cada dato con direccióndesde o hacia los esclavos, se representa por medio de una secuencia de 11 bits, 8 bits que corresponden al datoenviado y 3 bits para la detección de errores. Si el dato enviado proviene del maestro del robot, se lee como unbyte con signo (valores enteros desde -128 hasta 127), el cual corresponde a:
Una referencia de velocidad si se encuentra en el rango entre -100 y 100, inclusive.
Un comando de control para realizar pruebas de depuración, en caso contrario.
Si el dato proviene desde uno de los esclavos, se lee como un byte sin signo (valores enteros desde 0 hasta 255),correspondiente a un valor proporcional a la medición de la lectura de pulsos del encoder.
76
Hacía Esclavo
1
Desde Esclavo
1
Hacía Esclavo
2
Desde Esclavo
2
Hacía Esclavo
n
Desde Esclavo
n
Trama de Datos
Comunicación Módulo maestro del robot (Maestro) - Módulos de control de los motores (Esclavos)
n Número de esclavos
Datos desde el maestro hacía los esclavos
Datos desde los esclavos hacía el maestro
Figura 6.3: Trama de datos de comunicación del módulo maestro del robot
De esta forma, cada trama de comunicación contiene la información que se envia desde y hacia todos losesclavos, siendo cada esclavo el responsable de la lectura y escritura de sus datos en el momento adecuado. Cadaesclavo sincroniza su comunicación con la cuenta de los pulsos de reloj generados por el maestro, la cual es unaseñal común para todos. Una vez se inicia la comunicación, todos los esclavos reciben la misma trama de datos,sin embargo, solo leen los datos correspondientes a su número de esclavo e inician la escritura inmediatamentedespués que han leido el dato dirigido a ellos. Luego de haber recibido la información y escrito el dato a enviardentro de la trama, deben esperar hasta que la cuenta de pulsos de reloj marque el final de la comunicación ycontinuar con sus demás funciones programadas.
Al tener la misma trama para todos los esclavos e iniciar y terminar la comunicación en el mismo instante, sepuede garantizar que todos los motores reciban y controlen su velocidad de manera sincronizada y se mantengaun movimiento fluido del robot. De la misma forma, al garantizar que las lecturas de pulsos de los esclavoslleguen con la misma trama, se garantiza que el cálculo de la odometría en el maestro del robot tenga valorestomados con la misma referencia de tiempo en cada uno de los motores y acercarse a la realidad del movimientodel robot.
6.2.1. Trama de datos
Cada trama de datos consiste en un intercalamiento de los datos enviados por el maestro a cada uno de losesclavos, y la respuesta correspondiente de cada uno de ellos, una vez termina el dato del maestro. En la figura6.4 se observa el conteo de bits realizado por el maestro y cada uno de 3 esclavos en una trama de comunicación.El conteo en el flanco de subida de la señal corresponde al conteo de bits del maestro, mientras que el conteo enel flanco de bajada corresponde al conteo de bits de los esclavos. Esto es, debido a las restricciones de tiempovistas anteriormente, el maestro escribe y lee sus datos en los flancos de subida de la señal de reloj, mientrasque los esclavos hacen lo correspondiente en la señal de bajada, de forma que se garantiza medio ciclo de relojpara que cada bit se estabilice, antes de ser leido por el dispositivo correspondiente.
77
Figura 6.4: Comunicación para 3 esclavos
Este conteo de bits es la forma mediante la cual se realiza la sincronía en la trama de comunicación. Unavez el esclavo inicia el envío de una trama, éste y cada uno de los esclavos llevan el conteo interno de bits, locual indica el momento en el cual cada dispositivo debe estar en modo de lectura, en modo de escritura o elmomento en el cual cambiar la dirección de sus puertos según el modo deseado. En la figura 6.4, se muestra elmomento exacto del conteo en el cual se realiza la lectura o escritura correspondientes, la dirección de cada unode los puertos y los cambios de dicha dirección.
78
6.2.2. Cálculo de tiempos
Dentro de cada trama de comunicación, cada vez que el maestro inicia una transferencia de datos envíala referencia de la velocidad deseada hacia el primer módulo de los motores. Una vez se completan los 11bits correspondientes al dato enviado, el esclavo inmediatamente responde con el valor de la última lecturaacumulada de pulsos hacia el maestro. Terminado este proceso, se repite el mismo flujo de datos con el segundomódulo de control de motores, y sucecivamente se va completando la trama hasta el envío y recibo de datos delúltimo de los esclavos presentes en el robot. De este modo se tiene una trama con un total de 2n datos, siendo n elnúmero total de esclavos presentes en el robot, donde por cada 2 datos, además de los bits con la información y ladetección de errores, se cuentan adicionalmente: 1 bit de configuración cada vez que se inicia una comunicaciónhacia un esclavo, 1 bit cada vez que se cambia la dirección de la comunicación hacía cada esclavo, y 1 bit cadavez que se termina la comunicación desde un esclavo, para un total de 1+ 11+ 1+ 11+ 1 = 25 bits por cadaesclavo presente, es decir 25n bits por cada trama de datos, como se puede ver en el ejemplo de la figura 6.2.
De estos 25 bits, 16 corresponden a los datos desde o hacia los esclavos, de modo que el porcentaje deinformación enviado en cada trama corresponde al:
1625×100% = 64% (6.1)
de los datos transmitidos.Para este trabajo de grado se cuenta con 3 esclavos (motores) y una señal de reloj de 320µs, de donde se
tienen: 100 bits por cada trama completa de datos, y una duración total de:
75×320µs = 24ms (6.2)
por cada trama de comunicación, lo que corresponde a una velocidad de:
75bits24ms
= 3,125Kbits/s (6.3)
6.2.3. Detección de errores
Los 3 bits adicionales para la detección de errores, se ubican al final de cada dato enviado, ya sea queprovenga de un esclavo o vaya dirigido hacia uno. Estos bits reperesentan un número entero entre 0 y 7, quedebe corresponder al residuo módulo 8 de la cantidad de 1’s que contiene el dato enviado. Si en el receptor, estevalor no corresponde a dicho residuo calculado sobre el dato recibido, se considera un dato con la integridadalterada, y es desechado por el mismo. En la figura 6.5 se observa bit a bit una descripción gráfica de la detecciónde errores.
79
(mod 8)
Bit 1 Bit 2 Bit 3 Bit 4 Bit 5 Bit 6 Bit 7 Bit 8 Bit 9 Bit 10 Bit 11
Bit 1 Bit 2 Bit 3 Bit 4 Bit 5 Bit 6 Bit 7 Bit 8 Bit 9 Bit 10 Bit 11
Dato:
Detección de Errores:
Figura 6.5: Detección de errores
6.3. Comunicación inalámbrica
Cada vez que el módulo de acción cooperativa tiene datos sobre la posición actual del robot o de la posicióndel atractor, contruye una trama como la mostrada en la figura 6.6.
Encabezado Primer Byte
Posicion X
Segundo Byte
Posicion X
Trama de Datos
Comunicación Módulo de estrategia de acción cooperativa - Módulo Maestro de Robot
Primer Byte
Posicion Y
Segundo Byte
Posicion Y
Primer Byte
Posicion T
Segundo Byte
Posicion TFin de Trama
Figura 6.6: Trama de datos de comunicación inalámbrica
Esta trama se compone de 8 bytes sin signo, que incluyen:
1. Encabezado: Contiene el número del robot en el primer nibble del byte (un valor entre 0 y 4), y en elsegundo nibble, un identificador que indica el tipo de dato, como se muestra en la figura
Figura 6.7: Encabezado comunicación inalámbrica
80
Id. Robot indica el número identificador del robot al cual se envía la información.
Id. Dato indica el tipo de dato que se va a utilizar. En este caso:
• 01 Indica coordenadas de ubicación del móvil.
• 10 Indica coordenadas del punto atractor.
• 11 Indica comando de control.
Kicker y retenedor indica si el robot debe realizar funciones de pateo o control de pelota respectivamente,con los bits 7 y 8 del encabezado.
1. Cada dato de posición enviado corresponde a un valor de tipo entero, por lo cual, cada coordenada se envíaen dos bytes separados. De modo que, para obtener las coordenadas enviadas es necesario combinar elvalor de Primer byte y Segundo byte en un único valor entero. Estos valores corresponden a los bytes 2 y3 para la coordenada en X, 4 y 5 para la coordenada en Y, y 6 y 7 para coordenada en T.
2. Fin de trama: Corresponde a un valor que indica el final de la trama. Es un byte cuyo valor debe ser iguala 0xFD (en hexadecimal). Si el byte número 8 de la trama no corresponde con este valor, se consideraque la trama no es válida y se descarta.
81
Capítulo 7
Conclusiones.
Con respecto al sistema de motores y procesadores esclavos se modelaron los motores como un sistema deprimer orden, obteniendo buenos resultados para esta aplicación. Aunque el procesador no tenía buenas especi-ficaciones comparado con otros procesadores actuales, se logró optimizar y controlar la velocidad cumpliendolos requerimientos de tiempos para un buen desempeño en competencias como la robocopa.
En general los resultados reales con respecto a los que se esperaban por medio de la simulación y modeladomatemático fueron muy similares. Se notó mucho la mejora en comparación al programa que estaba anterior-mente. El control de posición funcionó dentro de los parámetros esperados, a pesar de que no es posible tener encuenta la totalidad de las variables que llegan a afectar al sistema, y cumplió con las expectativas de desempeñoal encontrarse ahora embarcado dentro de cada uno de los robots.
En cuanto a la odometría, se hizo necesario corregir las constantes teóricas calculadas, debido a factores queresultan muy imprácticos de modelar, como la masa del robot, la carga de las baterías, el patinaje de las ruedasy la exactitud en las medidas de los parámetros físicos. Con las correcciones se notó la mejora del sistema.
Se comprobó que utilizar un único sistema de localización no basta para un óptimo desempeño del control deposición. Por un lado, la odometría posee error acumulativo que causa que, en un tiempo no muy prolongado, sepierda la correcta ubicación del móvil. Por otro lado, la cámara está siempre expuesta a cambios en el ambientedel entorno de los robots, lo cual es causal de errores y medidas incorrerctas de la posición del robot, lo quegenera comportamientos indeseados. La implementación del filtro de kalman logra mitigar en gran medida elimpacto de ambas fuentes de error.
Aún cuando la comunicación propuesta para los módulos de control de los motores resultó satisfactoriaen este caso específico, es recomendable tener procesadores dentro de los cuales se disponga del hardwarenecesario para soportar algún protocolo de comunicaciones estándar, ya que esto representaría liberar una grancantidad carga de los procesadores, además de optimizar el tiempo de respuesta en la transmisión de datos.
El sistema de detección de errores demostró ser lo suficientemente confiable para detectar y eliminar el 100%de los paquetes defectuosos. Adicionalmente, los paquetes defectuosos, en ambos extremos de la comunicación,nunca superaron el umbral del 3% de la cantidad total de datos comunicados. Sin embargo, al ser un protocoloimplementado en su totalidad por software, y compartiendo tiempo de procesamiento con las demás tareas acumplir, resultó en una velocidad que no alcanza la velocidad estándar de una conexión UART (9600 bps).
82
Bibliografía
[1] Robocup, “Robocup.”
[2] M. Grupos de Investigación SIDRe, SIRP, GIAP, “Agentes Cooperativos: Cooperación en Sistemas Mul-tiagentes Aplicada a Robótica Móvil,” p. 579, 2006.
[3] K. Ogata, Sistemas de control en tiempo discreto. 1996.
[4] J. D. Muñoz Frías, “Sistemas empotrados en Tiempo Real,” in Sistemas empotrados en Tiempo Real, 2009.
[5] Robocup Support Team, “Laws of the RoboCup Small Size League 2012 LAW 1 - The Field of Play,”pp. 1–31, 2012.
[6] B. Barshan and H. Durrant-Whyte, “Inertial navigation systems for mobile robots,” IEEE Transactions on
Robotics and Automation, vol. 11, pp. 328–342, June 1995.
[7] H. Brugger and E. P. Hofer, “Odometry Error Correction by Sensor Fusion for Autonomous Mobile RobotNavigation Cristina Tar i n Sauer,” pp. 1654–1658, 2001.
[8] H. Safadi, “Local Path Planning Using Virtual Potential Field,” Spatial Representation and Mobile Robo-
tics Project, 2004.
[9] H. Choset, “Accurate relative localization using odometry,” 2003 IEEE International Conference on Ro-
botics and Automation (Cat. No.03CH37422), pp. 1606–1612, 2003.
[10] T. A. Baede, “Motion control of an omnidirectional mobile robot,” 2006.
[11] F. Lange and G. Hirzinger, “Learning Force Control with Position Controlled Robots,” no. April, pp. 2282–2288, 1996.
[12] MaxStream, “XBee / XBee-PRO OEM RF Modules,” no. 801, pp. 9–12, 2007.
[13] Y. Wang and K. Song, “A new approach to realize UART,” Proceedings of 2011 International Conference
on Electronic & Mechanical Engineering and Information Technology, pp. 2749–2752, Aug. 2011.
[14] R. Zurawski, INDUSTRIAL COMMUNICATION TECHNOLOGY. 2005.
[15] J. D. Munoz Frias, “Sistemas informáticos en tiempo real para sistemas de control,” Anales de mecánica y
electricidad, pp. 35–40, 2002.
[16] R. Barry, Using the freeRTOS Real Time Kernel. 2009.
83
[17] D. Knuth, “The Art Computer Programming,” in Fundamental Algorithms, Addison-Wesley, 3 ed., 1997.
[18] E. González, A. Perez, J. Cruz, and C. Bustacara, “MRCC: A Multi-Resolution Cooperative Control AgentArchitecture,” 2007 IEEE/WIC/ACM International Conference on Intelligent Agent Technology (IAT’07),pp. 391–394, 2007.
[19] J. Pablo, G. Ruiz, and G. Guerrero, “BESA / ME : multi  agent applications development platform overmobile devices with JME,” vol. 6, no. 3, 2009.
[20] E. Gonzalez, F. D. Rosa, A. S. Miranda, C. F. Rodríguez, and M. Manrique, “Bochica 2011 - Team Des-cription Paper,” pp. 1–9, 2011.
[21] R. Rojas and A. G. F, “Holonomic Control of a robot with an omni- directional drive .,” 2006.
[22] Robocupssl, “No Title.”
[23] Y. Liu and J.-h. Liu, “RoboCup is a Stage which impulse the Research of Basic Technology in Robot,”no. Fig 2, pp. 23–28, 2009.
84
Capítulo 8
Anexos
8.1. Soccer Small Leage (SSL)
La liga para robots pequeños, en adelante SSL (Small Size League), de la cual hacen parte los robots dearquitectura Agentcoop desarrollados en la Pontificia Universidad Javeriana de Bogotá, es una de las divisionesde la categoría RoboCupSoccer, competencia para robots autónomos jugadores de futbol. La SSL se centra enel problema de la cooperación multi-agente y el control, en un entorno muy dinámico con un sistema híbridocentralizado [22]. Este proyecto tiene como objetivo el desarrollo basado en las normas establecidas para laliga, en la cual los robots deben de cumplir la norma f-180, que establece que las medidas máximas de losrobots no pueden exceder un circulo de 18 cm y una altura de 15 cm, como se muestra en la figura 8.1, tomadadirectamente del reglamento de la liga.
Figura 8.1: Medidas máximas reglamentarias liga SSL [5]
En ésta liga se desarrollan los partidos más dinámicos e intensos de toda la competencia, debido a la velo-cidad de juego considerada en relación con las dimensiones del campo y la velocidad de desplazamiento de losrobots. Los robots SSL, en promedio, llegan a moverse a más de 2 m/s y la pelota llega a alcanzar una velocidadde hasta 15m/s en algunos casos extraordinarios.
En un partido SSL, dos equipos de 5 robots cada uno juegan fútbol sobre un tapete verde con medidas de6 metros de largo por 4 metros de ancho y una pelota de golf de color naranja. El objetivo del juego es, pormedio de las diferentes estrategías de acción cooperativa establecidas por cada equipo, lograr el mayor númerode goles en el arco contrario, cuyas medidas reglamentarias se observan en la figura 8.2.
85
Figura 8.2: Medidas reglamentarias del Arco, liga SSL
La cancha se encuentra dividida por una línea recta en dos mitades iguales, encargada de separar el campodonde cada equipo inicia un partido y sobre cada una de las cuales se encuentra ubicado el arco delimitado porun área en forma de semicírculo, la cual no pueden sobrepasar los arqueros de cada uno de los equipos. En lafigura 8.3 se muestran las medidas reglamentarias establecidas para el año 2012.
Figura 8.3: Dimensiones, en milímetros, de la cancha para la liga SSL
Existen dos tipos de robots para jugar en esta liga: se encuentran los que tienen cámara incorporada y losque tienen una o más cámaras globales que abarquen toda la cancha, siendo estos últimos los de interés para elproyecto [23]. En la actualidad la liga comtempla el uso de dos cámaras, ubicadas en cada una de las mediascanchas en las que se encuentra dividido el tapete. Cada una se ubica a 4 metros de altura, y su función es lade capturar y reconocer los patrones de identificación estandar que debe utilizar cada uno de los robots que seencuentren sobre el campo. Este patrón consta de cuatro círculos de colores ubicados al rededor de un círculoazul central, donde cada combinación de colores representa un identificador diferente para cada robot, como semuestra en la figura 8.4.
86
Figura 8.4: Patrón de colores para identificación de robots en liga SSL
El patrón de colores se debe ubicar sobre la solapa de cada uno de los robots, cubriendolos completamentelen su parte superior, cumpliendo con las medidas reglamentarias mostradas en la figura 8.5.
Figura 8.5: Medidas reglamentarias para el patrón de colores
8.2. Hardware
El proyecto para robots jugadores de futbol que funcionan sobre la plataforma robótica Agentcoop cuentanen la actualidad con todo el Hardware necesario para su funcionamiento, por lo cual dentro de las especificacio-nes y requerimientos del trabajo de grado se deben tener en cuenta los principales dispositivos sobre los cualesse viene soportando la arquitectura en su totalidad:
8.2.1. Módulo de Visión Artificial
2 Megapixeles
Tamaño 29 x 44 x 74 mm
Comunicación Firewire B - IEEE 1394b (800 Mb/s)
87
LUTs, corrección de color y sombras
61 fps (frames per second)
12Voltios, 3Watts
8.2.2. Módulo de Estrategia de acción cooperativa
Procesador Intel Xeon 2.66GHz
2MB de memoria RAM
Ubuntu 11.04 Natty Narwhal
Dos tarjeta Firewire de 9 a 9 con 4 puertos cada una
Netbeans 7.0.1
SSL-Vision System
8.2.3. Módulo Maestro del Robot
Procesador módulo maestro del robotDsPIC30f6010A:
Procesador de 16 bits.
Reloj de 12 MHz.
5 Timers de 16 bits.
2 Módulos de comunicación UART.
Módulo de comunicacion I2C.
Módulo de comunicación SPI.
Módulo QEI (Quadrature Encoder Interface).
8 salidas de PWM.
Módulo de comunicación inalámbrica
Velocidad máxima de transmisión: 115200 bps (bits per second).
Frecuencia de operación 2.4 GHz.
Potencia de transmisión 1mW
Rango de operación de 100m.
Antena incorporada.
Adicional 7 entradas de ADC de 10 bits y 8 pines de entrada/salida digitales.
88
8.2.4. Módulo de Control de los Motores
Procesador módulo de control de los motoresPIC16HV610:
Procesador de 8 bits.
Reloj de 8MHz.
1 Timer de 8 bits.
1 Timer de 16 bits.
4 salidas de PWM.
Motores
3 Motores HN-GH12-1634T
12Vdc - 200 RPM con 30:1
Motoreductor modificado con relación de giro 10 a 1.
Velocidad nominal sin carga 600RPM ∼ 157cm/s
Encoder
Encoder incremental con dos canales
Control de Giro
Puente H MOSFET: ZXMHC3f381N8.
Rueda
Rueda omnidireccional S360162.
89